From 6d776812d3de28a42de33321d240f7538960813f Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 26 Jun 2012 18:52:27 +0000 Subject: [PATCH 02/21] new wrap! :) --- .cproject | 1 + gtsam.h | 47 +++---- gtsam/linear/NoiseModel.h | 2 +- matlab/examples/OdometryExample.m | 3 +- wrap/Argument.cpp | 6 +- wrap/Argument.h | 2 + wrap/Class.cpp | 57 ++++---- wrap/Class.h | 5 +- wrap/Constructor.cpp | 81 ++++++++++-- wrap/Constructor.h | 17 ++- wrap/Method.cpp | 17 ++- wrap/Module.cpp | 29 +++-- wrap/ReturnValue.cpp | 44 ++++--- wrap/StaticMethod.cpp | 12 +- wrap/matlab.h | 209 +++++++++++------------------- wrap/utilities.cpp | 13 +- wrap/utilities.h | 5 +- 17 files changed, 312 insertions(+), 238 deletions(-) diff --git a/.cproject b/.cproject index c593a801b..7828f8165 100644 --- a/.cproject +++ b/.cproject @@ -2325,4 +2325,5 @@ + diff --git a/gtsam.h b/gtsam.h index 68ed6201d..941e39af7 100644 --- a/gtsam.h +++ b/gtsam.h @@ -497,7 +497,7 @@ class SimpleCamera { //************************************************************************* // inference //************************************************************************* -class Permutation { +/*class Permutation { // Standard Constructors and Named Constructors Permutation(); Permutation(size_t nVars); @@ -511,12 +511,12 @@ class Permutation { bool equals(const gtsam::Permutation& rhs, double tol) const; // Standard interface - size_t at(size_t variable) const; + //size_t at(size_t variable) const; size_t size() const; bool empty() const; void resize(size_t newSize); - gtsam::Permutation* permute(const gtsam::Permutation& permutation) const; - gtsam::Permutation* inverse() const; + //gtsam::Permutation permute(const gtsam::Permutation& permutation) const; + //gtsam::Permutation inverse() const; }; class IndexFactor { @@ -541,24 +541,24 @@ class IndexFactor { class IndexConditional { // Standard Constructors and Named Constructors - IndexConditional(); - IndexConditional(size_t key); - IndexConditional(size_t key, size_t parent); - IndexConditional(size_t key, size_t parent1, size_t parent2); - IndexConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); + //IndexConditional(); + //IndexConditional(size_t key); + //IndexConditional(size_t key, size_t parent); + //IndexConditional(size_t key, size_t parent1, size_t parent2); + //IndexConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); // FIXME: Must wrap std::vector for this to work //IndexFactor(size_t key, const std::vector& parents); //IndexConditional(const std::vector& keys, size_t nrFrontals); //template static shared_ptr FromKeys(const KEYS& keys, size_t nrFrontals); // Testable - void print(string s) const; - bool equals(const gtsam::IndexConditional& other, double tol) const; + //void print(string s) const; + //bool equals(const gtsam::IndexConditional& other, double tol) const; // Standard interface - size_t nrFrontals() const; - size_t nrParents() const; - gtsam::IndexFactor* toFactor() const; + //size_t nrFrontals() const; + //size_t nrParents() const; + //gtsam::IndexFactor* toFactor() const; }; #include @@ -672,7 +672,7 @@ class VariableIndex { size_t nFactors() const; size_t nEntries() const; void permute(const gtsam::Permutation& permutation); -}; +};*/ //************************************************************************* // linear @@ -681,11 +681,12 @@ class VariableIndex { #include namespace noiseModel { class Base { + void print(string s) const; }; class Gaussian { - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + //static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); + //static gtsam::noiseModel::Gaussian* Covariance(Matrix R); // Matrix R() const; // FIXME: cannot parse!!! void print(string s) const; }; @@ -712,7 +713,7 @@ class Unit { }///\namespace noiseModel -class Sampler { +/*class Sampler { Sampler(gtsam::noiseModel::Diagonal* model, int seed); Sampler(Vector sigmas, int seed); Sampler(int seed); @@ -723,7 +724,7 @@ class Sampler { Vector sample(); Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); -}; +};*/ class VectorValues { VectorValues(); @@ -897,11 +898,11 @@ class Ordering { // Standard interface size_t nVars() const; size_t size() const; - size_t at(size_t key) const; + size_t at(size_t key); bool exists(size_t key) const; void insert(size_t key, size_t order); void push_back(size_t key); - void permuteWithInverse(const gtsam::Permutation& inversePermutation); + //void permuteWithInverse(const gtsam::Permutation& inversePermutation); // FIXME: Wrap InvertedMap as well //InvertedMap invert() const; }; @@ -958,9 +959,9 @@ class Marginals { #include class LevenbergMarquardtParams { - LevenbergMarquardtParams(); + /*LevenbergMarquardtParams(); LevenbergMarquardtParams(double initial, double factor, double bound, size_t verbose); - void print(string s) const; + void print(string s) const;*/ double getlambdaInitial() const ; double getlambdaFactor() const ; double getlambdaUpperBound() const; diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index c67828822..9d39b5674 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -262,7 +262,7 @@ namespace gtsam { * A diagonal noise model created by specifying a Vector of sigmas, i.e. * standard devations, the diagonal of the square root covariance matrix. */ - static shared_ptr Sigmas(const Vector& sigmas, bool smart = true); + static shared_ptr Sigmas(const Vector& sigmas, bool smart = false); /** * A diagonal noise model created by specifying a Vector of variances, i.e. diff --git a/matlab/examples/OdometryExample.m b/matlab/examples/OdometryExample.m index f203c858e..8b2439c78 100644 --- a/matlab/examples/OdometryExample.m +++ b/matlab/examples/OdometryExample.m @@ -29,6 +29,7 @@ odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x graph.addRelativePose(1, 2, odometry, odometryNoise); graph.addRelativePose(2, 3, odometry, odometryNoise); +pause %% print graph.print(sprintf('\nFactor graph:\n')); @@ -56,4 +57,4 @@ for i=1:result.size() end axis([-0.6 4.8 -1 1]) axis equal -view(2) \ No newline at end of file +view(2) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index f51b86864..e2073ac8c 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -12,6 +12,7 @@ /** * @file Argument.ccp * @author Frank Dellaert + * @author Andrew Melim **/ #include @@ -53,14 +54,14 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { 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< "; + file.oss << " " << cppType << "& " << name << " = *unwrap_shared_ptr< "; else // Not a pointer or a reference: emit an "unwrap" call // unwrap is specified in matlab.h as a series of template specializations // that know how to unpack the expected MATLAB object // example: double tol = unwrap< double >(in[2]); // example: Vector v = unwrap< Vector >(in[1]); - file.oss << cppType << " " << name << " = unwrap< "; + file.oss << " " << cppType << " " << name << " = unwrap< "; file.oss << cppType << " >(" << matlabName; if (is_ptr || is_ref) file.oss << ", \"" << matlabType << "\""; @@ -126,3 +127,4 @@ void ArgumentList::matlab_unwrap(FileWriter& file, int start) const { } /* ************************************************************************* */ + diff --git a/wrap/Argument.h b/wrap/Argument.h index 000e3079a..b4c3fe4a7 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -13,6 +13,7 @@ * @file Argument.h * @brief arguments to constructors and methods * @author Frank Dellaert + * @author Andrew Melim **/ #pragma once @@ -65,6 +66,7 @@ struct ArgumentList: public std::vector { * @param start initial index for input array, set to 1 for method */ void matlab_unwrap(FileWriter& file, int start = 0) const; // MATLAB to C++ + }; } // \namespace wrap diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 6f6f91b0a..7280d5488 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -10,8 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file Class.ccp + * @file Class.cpp * @author Frank Dellaert + * @author Andrew Melim **/ #include @@ -22,6 +23,7 @@ #include "Class.h" #include "utilities.h" +#include "Argument.h" using namespace std; using namespace wrap; @@ -43,13 +45,24 @@ void Class::matlab_proxy(const string& classFile) const { file.oss << " methods" << endl; // constructor file.oss << " function obj = " << matlabName << "(varargin)" << endl; - BOOST_FOREACH(Constructor c, constructors) - c.matlab_proxy_fragment(file,matlabName); - file.oss << " if nargin ~= 13 && obj.self == 0, error('" << matlabName << " constructor failed'); end" << endl; + //i is constructor id + int id = 0; + BOOST_FOREACH(ArgumentList a, constructor.args_list) + { + constructor.matlab_proxy_fragment(file,matlabName, id, a); + id++; + } + //Static constructor collect call + file.oss << " if nargin ==14, new_" << matlabName << "_(varargin{1},0); end" << endl; + file.oss << " if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('" << matlabName << " constructor failed'); end" << endl; file.oss << " end" << endl; // deconstructor file.oss << " function delete(obj)" << endl; - file.oss << " delete_" << matlabName << "(obj);" << endl; + file.oss << " if obj.self ~= 0" << endl; + file.oss << " fprintf(1,'MATLAB class deleting %x',obj.self);" << endl; + file.oss << " new_" << matlabName << "_(obj.self);" << endl; + file.oss << " obj.self = 0;" << endl; + file.oss << " end" << endl; file.oss << " end" << endl; file.oss << " function display(obj), obj.print(''); end" << endl; file.oss << " function disp(obj), obj.display; end" << endl; @@ -61,18 +74,19 @@ void Class::matlab_proxy(const string& classFile) const { } /* ************************************************************************* */ +//TODO: Consolidate into single file void Class::matlab_constructors(const string& toolboxPath) const { - BOOST_FOREACH(Constructor c, constructors) { - c.matlab_mfile (toolboxPath, qualifiedName()); - c.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), using_namespaces, includes); + /*BOOST_FOREACH(Constructor c, constructors) { + args_list.push_back(c.args); + }*/ + + BOOST_FOREACH(ArgumentList a, constructor.args_list) { + constructor.matlab_mfile(toolboxPath, qualifiedName(), a); } + constructor.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), + using_namespaces, includes); } -/* ************************************************************************* */ -void Class::matlab_deconstructor(const string& toolboxPath) const { - d.matlab_mfile (toolboxPath, qualifiedName()); - d.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), using_namespaces, includes); -} /* ************************************************************************* */ void Class::matlab_methods(const string& classPath) const { string matlabName = qualifiedName(), cppName = qualifiedName("::"); @@ -97,9 +111,7 @@ void Class::matlab_make_fragment(FileWriter& file, const string& mexFlags) const { string mex = "mex " + mexFlags + " "; string matlabClassName = qualifiedName(); - BOOST_FOREACH(Constructor c, constructors) - file.oss << mex << c.matlab_wrapper_name(matlabClassName) << ".cpp" << endl; - file.oss << mex << d.matlab_wrapper_name(matlabClassName) << ".cpp" << endl; + file.oss << mex << constructor.matlab_wrapper_name(matlabClassName) << ".cpp" << endl; BOOST_FOREACH(StaticMethod sm, static_methods) file.oss << mex << matlabClassName + "_" + sm.name << ".cpp" << endl; file.oss << endl << "cd @" << matlabClassName << endl; @@ -123,15 +135,12 @@ void Class::makefile_fragment(FileWriter& file) const { // // Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING) - string matlabName = qualifiedName(); + string matlabName = qualifiedName(); - // collect names - vector file_names; - BOOST_FOREACH(Constructor c, constructors) { - string file_base = c.matlab_wrapper_name(matlabName); - file_names.push_back(file_base); - } - file_names.push_back(d.matlab_wrapper_name(matlabName)); + // collect names + vector file_names; + string file_base = constructor.matlab_wrapper_name(matlabName); + file_names.push_back(file_base); BOOST_FOREACH(StaticMethod c, static_methods) { string file_base = matlabName + "_" + c.name; file_names.push_back(file_base); diff --git a/wrap/Class.h b/wrap/Class.h index 875b3203e..fb9906716 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -13,6 +13,7 @@ * @file Class.h * @brief describe the C++ class that is being wrapped * @author Frank Dellaert + * @author Andrew Melim **/ #pragma once @@ -33,19 +34,17 @@ struct Class { // Then the instance variables are set directly by the Module constructor std::string name; ///< Class name - std::vector constructors; ///< Class constructors std::vector methods; ///< Class methods std::vector static_methods; ///< Static methods std::vector namespaces; ///< Stack of namespaces std::vector using_namespaces; ///< default namespaces std::vector includes; ///< header include overrides - Deconstructor d; + Constructor constructor; ///< Class constructors bool verbose_; ///< verbose flag // And finally MATLAB code is emitted, methods below called by Module::matlab_code void matlab_proxy(const std::string& classFile) const; ///< emit proxy class void matlab_constructors(const std::string& toolboxPath) const; ///< emit constructor wrappers - void matlab_deconstructor(const std::string& toolboxPath) const; void matlab_methods(const std::string& classPath) const; ///< emit method wrappers void matlab_static_methods(const std::string& classPath) const; ///< emit static method wrappers void matlab_make_fragment(FileWriter& file, diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index fda7db1b4..0ee459389 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -12,10 +12,12 @@ /** * @file Constructor.ccp * @author Frank Dellaert + * @author Andrew Melim **/ #include #include +#include #include @@ -25,14 +27,16 @@ using namespace std; using namespace wrap; + /* ************************************************************************* */ string Constructor::matlab_wrapper_name(const string& className) const { - string str = "new_" + className + "_" + args.signature(); + string str = "new_" + className + "_"; return str; } /* ************************************************************************* */ -void Constructor::matlab_proxy_fragment(FileWriter& file, const string& className) const { +void Constructor::matlab_proxy_fragment(FileWriter& file, + const string& className, const int id, const ArgumentList args) const { size_t nrArgs = args.size(); // check for number of arguments... file.oss << " if (nargin == " << nrArgs; @@ -45,19 +49,17 @@ void Constructor::matlab_proxy_fragment(FileWriter& file, const string& classNam first=false; } // emit code for calling constructor - file.oss << "), obj.self = " << matlab_wrapper_name(className) << "("; + file.oss << "), obj.self = " << matlab_wrapper_name(className) << "(" << "0," << id; // emit constructor arguments - first = true; for(size_t i=0;i& using_namespaces, const vector& includes) const { + const vector& using_namespaces, + const vector& includes) const { string matlabName = matlab_wrapper_name(matlabClassName); // open destination wrapperFile @@ -91,12 +94,66 @@ void Constructor::matlab_wrapper(const string& toolboxPath, generateIncludes(file, name, includes); generateUsingNamespace(file, using_namespaces); + //Typedef boost::shared_ptr + file.oss << "typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; + file.oss << endl; + + //Generate collector + file.oss << "static std::set collector;" << endl; + file.oss << endl; + + //Generate the destructor function + file.oss << "struct Destruct" << endl; + file.oss << "{" << endl; + file.oss << " void operator() (Shared* p)" << endl; + file.oss << " {" << endl; + file.oss << " delete p;" << endl; + file.oss << " }" << endl; + file.oss << "};" << endl; + file.oss << endl; + + //Generate cleanup function + file.oss << "void cleanup(void) {" << endl; + file.oss << " mexPrintf(\"MEX-file new_gtsamDummy_ is terminating\");" << endl; + file.oss << " std::for_each( collector.begin(), collector.end(), Destruct() );" << endl; + file.oss << "}" << endl; + file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl; file.oss << "{" << endl; - file.oss << " checkArguments(\"" << matlabName << "\",nargout,nargin," << args.size() << ");" << endl; - args.matlab_unwrap(file); // unwrap arguments - file.oss << " " << cppClassName << "* self = new " << cppClassName << "(" << args.names() << ");" << endl; // need qualified name, delim: "::" - file.oss << " out[0] = wrap_constructed(self,\"" << matlabClassName << "\");" << endl; // need matlab qualified name + //Cleanup function callback + file.oss << " mexAtExit(cleanup);" << endl; + file.oss << endl; + file.oss << " const mxArray* input = in[0];" << endl; + file.oss << " Shared* self = *(Shared**) mxGetData(input);" << endl; + file.oss << endl; + file.oss << " if(self) {" << endl; + file.oss << " if(nargin > 1) {" << endl; + file.oss << " collector.insert(self);" << endl; + file.oss << " std::cout << \"Collected\" << collector.size() << std::endl;" << endl; + file.oss << " }" << endl; + file.oss << " else if(collector.erase(self))" << endl; + file.oss << " delete self;" << endl; + file.oss << " } else {" << endl; + file.oss << " int nc = unwrap(in[1]);" << endl; + file.oss << " mexPrintf(\"MEX-file constructor\");" << endl << endl;; + + int i = 0; + BOOST_FOREACH(ArgumentList al, args_list) + { + file.oss << " if(nc == " << i <<") {" << endl; + al.matlab_unwrap(file, 2); // unwrap arguments, start at 1 + file.oss << " self = new Shared(new " << cppClassName << "(" << al.names() << "));" << endl; + file.oss << " }" << endl; + i++; + } + + //file.oss << " self = construct(nc, in);" << endl; + file.oss << " collector.insert(self);" << endl; + file.oss << " std::cout << \"constructed \" << self << \", size=\" << collector.size() << std::endl;" << endl; + file.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);" << endl; + file.oss << " *reinterpret_cast (mxGetPr(out[0])) = self;" << endl; + file.oss << " }" << endl; + file.oss << "}" << endl; // close file diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 4aba5b984..a766905b6 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -33,7 +33,8 @@ struct Constructor { } // Then the instance variables are set directly by the Module constructor - ArgumentList args; + // TODO:Vector of argument lists? + std::vector args_list; std::string name; bool verbose_; @@ -48,11 +49,14 @@ struct Constructor { * Create fragment to select constructor in proxy class, e.g., * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end */ - void matlab_proxy_fragment(FileWriter& file, const std::string& className) const; + void matlab_proxy_fragment(FileWriter& file, + const std::string& className, const int i, + const ArgumentList args) const; /// m-file void matlab_mfile(const std::string& toolboxPath, - const std::string& qualifiedMatlabName) const; + const std::string& qualifiedMatlabName, + const ArgumentList args) const; /// cpp wrapper void matlab_wrapper(const std::string& toolboxPath, @@ -60,7 +64,12 @@ struct Constructor { const std::string& matlabClassName, const std::vector& using_namespaces, const std::vector& includes) const; + + /// constructor function + void generate_construct(FileWriter& file, const std::string& cppClassName, + std::vector& args_list) const; + }; -} // \namespace wrap +} // \namespace wrap diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 7686ecd2d..ccd36fe50 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -61,6 +61,15 @@ void Method::matlab_wrapper(const string& classPath, generateIncludes(file, className, includes); generateUsingNamespace(file, using_namespaces); + if(returnVal.isPair) + { + file.oss << "typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; + file.oss << "typedef boost::shared_ptr<" << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 << ";"<< endl; + } + else + file.oss << "typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; + + file.oss << "typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; // call file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start @@ -73,9 +82,9 @@ void Method::matlab_wrapper(const string& classPath, // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - file.oss << " boost::shared_ptr<" << cppClassName << "> self = unwrap_shared_ptr< " << cppClassName - << " >(in[0],\"" << matlabClassName << "\");" << endl; - + file.oss << " mxArray* mxh = mxGetProperty(in[0],0,\"self\");" << endl; + file.oss << " Shared* self = *reinterpret_cast (mxGetPr(mxh));" << endl; + file.oss << " Shared obj = *self;" << endl; // unwrap arguments, see Argument.cpp args.matlab_unwrap(file,1); @@ -84,7 +93,7 @@ void Method::matlab_wrapper(const string& classPath, file.oss << " "; if (returnVal.type1!="void") file.oss << returnVal.return_type(true,ReturnValue::pair) << " result = "; - file.oss << "self->" << name << "(" << args.names() << ");\n"; + file.oss << "obj->" << name << "(" << args.names() << ");\n"; // wrap result // example: out[0]=wrap(result); diff --git a/wrap/Module.cpp b/wrap/Module.cpp index e0771351a..972bacbee 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -26,8 +26,10 @@ #include #include #include +#include #include +#include using namespace std; using namespace wrap; @@ -52,8 +54,9 @@ Module::Module(const string& interfacePath, ReturnValue retVal0, retVal; Argument arg0, arg; ArgumentList args0, args; + vector arg_dup; ///keep track of duplicates Constructor constructor0(enable_verbose), constructor(enable_verbose); - Deconstructor deconstructor0(enable_verbose), deconstructor(enable_verbose); + //Deconstructor deconstructor0(enable_verbose), deconstructor(enable_verbose); Method method0(enable_verbose), method(enable_verbose); StaticMethod static_method0(enable_verbose), static_method(enable_verbose); Class cls0(enable_verbose),cls(enable_verbose); @@ -118,11 +121,12 @@ Module::Module(const string& interfacePath, Rule constructor_p = (className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p) - [assign_a(constructor.args,args)] - [assign_a(constructor.name,cls.name)] - [assign_a(args,args0)] - [push_back_a(cls.constructors, constructor)] - [assign_a(constructor,constructor0)]; + [push_back_a(constructor.args_list, args)] + [assign_a(args,args0)]; + //[assign_a(constructor.args,args)] + //[assign_a(constructor.name,cls.name)] + //[push_back_a(cls.constructors, constructor)] + //[assign_a(constructor,constructor0)]; Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::"); @@ -184,13 +188,16 @@ Module::Module(const string& interfacePath, >> '{' >> *(functions_p | comments_p) >> str_p("};")) + [assign_a(constructor.name, cls.name)] + [assign_a(cls.constructor, constructor)] [assign_a(cls.namespaces, namespaces)] [assign_a(cls.using_namespaces, using_namespace_current)] [append_a(cls.includes, namespace_includes)] - [assign_a(deconstructor.name,cls.name)] - [assign_a(cls.d, deconstructor)] + //[assign_a(deconstructor.name,cls.name)] + //[assign_a(cls.d, deconstructor)] [push_back_a(classes,cls)] - [assign_a(deconstructor,deconstructor0)] + //[assign_a(deconstructor,deconstructor0)] + [assign_a(constructor, constructor0)] [assign_a(cls,cls0)]; Rule namespace_def_p = @@ -336,7 +343,7 @@ void Module::matlab_code(const string& mexCommand, const string& toolboxPath, cls.matlab_proxy(classFile); // verify all of the function arguments - verifyArguments(validTypes, cls.constructors); + //TODO:verifyArguments(validTypes, cls.constructor.args_list); verifyArguments(validTypes, cls.static_methods); verifyArguments(validTypes, cls.methods); @@ -350,7 +357,7 @@ void Module::matlab_code(const string& mexCommand, const string& toolboxPath, cls.matlab_methods(classPath); // create deconstructor - cls.matlab_deconstructor(toolboxPath); + //cls.matlab_deconstructor(toolboxPath); // add lines to make m-file makeModuleMfile.oss << "%% " << cls.qualifiedName() << endl; diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index ace1159ec..087a1c6ef 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -3,6 +3,7 @@ * * @date Dec 1, 2011 * @author Alex Cunningham + * @author Andrew Melim */ #include @@ -17,11 +18,11 @@ using namespace wrap; string ReturnValue::return_type(bool add_ptr, pairing p) const { if (p==pair && isPair) { string str = "pair< " + - maybe_shared_ptr(add_ptr && isPtr1, qualifiedType1("::")) + ", " + - maybe_shared_ptr(add_ptr && isPtr2, qualifiedType2("::")) + " >"; + maybe_shared_ptr(add_ptr && isPtr1, qualifiedType1("::"), type1) + ", " + + maybe_shared_ptr(add_ptr && isPtr2, qualifiedType2("::"), type2) + " >"; return str; } else - return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::")); + return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::"), (p==arg2)? type2 : type1); } /* ************************************************************************* */ @@ -44,31 +45,44 @@ string ReturnValue::qualifiedType2(const string& delim) const { } /* ************************************************************************* */ +//TODO:Fix this void ReturnValue::wrap_result(FileWriter& file) const { string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1(); string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2(); if (isPair) { // first return value in pair - if (isPtr1) // if we already have a pointer - file.oss << " out[0] = wrap_shared_ptr(result.first,\"" << matlabType1 << "\");\n"; - else if (category1 == ReturnValue::CLASS) // if we are going to make one - file.oss << " out[0] = wrap_shared_ptr(boost::make_shared< " << cppType1 << " >(result.first),\"" << matlabType1 << "\");\n"; + if (isPtr1) {// if we already have a pointer + file.oss << " Shared" << type1 <<"* ret = new Shared" << type1 << "(result.first);" << endl; + file.oss << " out[0] = wrap_collect_shared_ptr(ret,\"" << matlabType1 << "\");\n"; + } + else if (category1 == ReturnValue::CLASS) { // if we are going to make one + file.oss << " Shared" << type1 << "* ret = new Shared" << type1 << "(new " << cppType1 << "(result.first));\n"; + file.oss << " out[0] = wrap_collect_shared_ptr(ret,\"" << matlabType1 << "\");\n"; + } else // if basis type file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n"; // second return value in pair - if (isPtr2) // if we already have a pointer - file.oss << " out[1] = wrap_shared_ptr(result.second,\"" << matlabType2 << "\");\n"; - else if (category2 == ReturnValue::CLASS) // if we are going to make one - file.oss << " out[1] = wrap_shared_ptr(boost::make_shared< " << cppType2 << " >(result.second),\"" << matlabType2 << "\");\n"; + if (isPtr2) {// if we already have a pointer + file.oss << " Shared" << type2 <<"* ret = new Shared" << type2 << "(result.second);" << endl; + file.oss << " out[1] = wrap_collect_shared_ptr(ret,\"" << matlabType2 << "\");\n"; + } + else if (category2 == ReturnValue::CLASS) { // if we are going to make one + file.oss << " Shared" << type2 << "* ret = new Shared" << type2 << "(new " << cppType2 << "(result.first));\n"; + file.oss << " out[0] = wrap_collect_shared_ptr(ret,\"" << matlabType2 << "\");\n"; + } else file.oss << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n"; } - else if (isPtr1) - file.oss << " out[0] = wrap_shared_ptr(result,\"" << matlabType1 << "\");\n"; - else if (category1 == ReturnValue::CLASS) - file.oss << " out[0] = wrap_shared_ptr(boost::make_shared< " << cppType1 << " >(result),\"" << matlabType1 << "\");\n"; + else if (isPtr1){ + file.oss << " Shared" << type1 <<"* ret = new Shared" << type1 << "(result);" << endl; + file.oss << " out[0] = wrap_collect_shared_ptr(ret,\"" << matlabType1 << "\");\n"; + } + else if (category1 == ReturnValue::CLASS){ + file.oss << " Shared" << type1 << "* ret = new Shared" << type1 << "(new " << cppType1 << "(result));\n"; + file.oss << " out[0] = wrap_collect_shared_ptr(ret,\"" << matlabType1 << "\");\n"; + } else if (matlabType1!="void") file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n"; } diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 7bdba650b..a4fb3fbe8 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -10,8 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file Method.ccp + * @file StaticMethod.ccp * @author Frank Dellaert + * @author Andrew Melim **/ #include @@ -62,6 +63,15 @@ void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& class generateIncludes(file, className, includes); generateUsingNamespace(file, using_namespaces); + if(returnVal.isPair) + { + file.oss << "typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; + file.oss << "typedef boost::shared_ptr<" << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 << ";"<< endl; + } + else + file.oss << "typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; + + file.oss << "typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; // call file.oss << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; // start diff --git a/wrap/matlab.h b/wrap/matlab.h index 313cfaa9f..99dd0c6b3 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -41,6 +41,8 @@ extern "C" { #include #include #include +#include +#include using namespace std; using namespace boost; // not usual, but for conciseness of generated code @@ -311,6 +313,77 @@ gtsam::Matrix unwrap< gtsam::Matrix >(const mxArray* array) { return A; } +/* + [create_object] creates a MATLAB proxy class object with a mexhandle + in the self property. Matlab does not allow the creation of matlab + objects from within mex files, hence we resort to an ugly trick: we + invoke the proxy class constructor by calling MATLAB, and pass 13 + dummy arguments to let the constructor know we want an object without + the self property initialized. We then assign the mexhandle to self. +*/ +// TODO: think about memory +mxArray* create_object(const char *classname, mxArray* h) { + mxArray *result; + mxArray* dummy[13] = {h,h,h,h,h, h,h,h,h,h, h,h,h}; + mexCallMATLAB(1,&result,13,dummy,classname); + mxSetProperty(result, 0, "self", h); + return result; +} + +/* + * Similar to create object, this also collects the shared_ptr in addition + * to creating the dummy object. Mainly used for static constructor methods + * which don't have direct access to the function. + */ +mxArray* create_collect_object(const char *classname, mxArray* h){ + mxArray *result; + //First arg is a flag to collect + mxArray* dummy[14] = {h,h,h,h,h, h,h,h,h,h, h,h,h,h}; + mexCallMATLAB(1,&result,14,dummy,classname); + mxSetProperty(result, 0, "self", h); + cout << "Return collect" << endl; + return result; +} + +/* + When the user calls a method that returns a shared pointer, we create + an ObjectHandle from the shared_pointer and return it as a proxy + class to matlab. +*/ +template +mxArray* wrap_shared_ptr(boost::shared_ptr< Class >* shared_ptr, const char *classname) { + mxArray* mxh = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast**> (mxGetPr(mxh)) = shared_ptr; + cout << "wrapped:" << mxh << endl << "end wrap" << endl; + //return mxh; + return create_object(classname, mxh); +} + +template +mxArray* wrap_collect_shared_ptr(boost::shared_ptr< Class >* shared_ptr, const char *classname) { + mxArray* mxh = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast**> (mxGetPr(mxh)) = shared_ptr; + cout << "wrapped:" << mxh << endl << "end wrap" << endl; + //return mxh; + return create_collect_object(classname, mxh); +} + +template +boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& className) { + cout << "UNWRAP CALL" << endl; + + mxArray* mxh = mxGetProperty(obj,0,"self"); + if (mxGetClassID(mxh) != mxUINT32OR64_CLASS || mxIsComplex(mxh) + || mxGetM(mxh) != 1 || mxGetN(mxh) != 1) error( + "Parameter is not an Shared type."); + + cout << "unwrapped:" << mxh << endl; + boost::shared_ptr* spp = *reinterpret_cast**> (mxGetPr(mxh)); + cout << "unwrapped:" << spp << endl; + return *spp; +} + + //***************************************************************************** // Shared Pointer Handle // inspired by mexhandle, but using shared_ptr @@ -329,6 +402,9 @@ public: ObjectHandle(T* ptr) : type(&typeid(T)), t(ptr) { signature = this; + mexPrintf("Created Shared Pointer use_count = %li\n", t.use_count()); + mexPrintf("Created Pointer points to %d\n", t.get()); + } // Constructor for shared pointers @@ -336,6 +412,8 @@ public: ObjectHandle(boost::shared_ptr shared_ptr) : /*type(&typeid(T)),*/ t(shared_ptr) { signature = this; + mexPrintf("Created sp from sp use_count = %li\n", t.use_count()); + mexPrintf("Created sp from sp points to %d\n", t.get()); } ~ObjectHandle() { @@ -418,33 +496,7 @@ mxArray* wrap_constructed(Class* pointer, const char *classname) { return handle->to_mex_handle(); } -/* - [create_object] creates a MATLAB proxy class object with a mexhandle - in the self property. Matlab does not allow the creation of matlab - objects from within mex files, hence we resort to an ugly trick: we - invoke the proxy class constructor by calling MATLAB, and pass 13 - dummy arguments to let the constructor know we want an object without - the self property initialized. We then assign the mexhandle to self. -*/ -// TODO: think about memory -mxArray* create_object(const char *classname, mxArray* h) { - mxArray *result; - mxArray* dummy[13] = {h,h,h,h,h, h,h,h,h,h, h,h,h}; - mexCallMATLAB(1,&result,13,dummy,classname); - mxSetProperty(result, 0, "self", h); - return result; -} -/* - When the user calls a method that returns a shared pointer, we create - an ObjectHandle from the shared_pointer and return it as a proxy - class to matlab. -*/ -template -mxArray* wrap_shared_ptr(boost::shared_ptr< Class > shared_ptr, const char *classname) { - ObjectHandle* handle = new ObjectHandle(shared_ptr); - return create_object(classname,handle->to_mex_handle()); -} //***************************************************************************** // unwrapping a MATLAB proxy class to a C++ object reference @@ -457,116 +509,11 @@ mxArray* wrap_shared_ptr(boost::shared_ptr< Class > shared_ptr, const char *clas the ObjectHandle from the self property, and returns a shared pointer to the object. */ -template -boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& className) { - #ifndef UNSAFE_WRAP - // Useful code to check argument type - // Problem, does not support inheritance - bool isClass = mxIsClass(obj, className.c_str()); - if (!isClass) { - mexPrintf("Expected %s, got %s\n", className.c_str(), mxGetClassName(obj)); - error("Argument has wrong type."); - } -#endif - mxArray* mxh = mxGetProperty(obj,0,"self"); - if (mxh==NULL) error("unwrap_reference: invalid wrap object"); - ObjectHandle* handle = ObjectHandle::from_mex_handle(mxh); - return handle->get_object(); -} -/* - * Specialized template for noise model. Checking their derived types properly - */ -// Isotropic -template <> -boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& className) { -#ifndef UNSAFE_WRAP - bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic"); - bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit"); - if (!isIsotropic && !isUnit) { - mexPrintf("Expected gtsamnoiseModelIsotropic or derived classes, got %s\n", mxGetClassName(obj)); - error("Argument has wrong type."); - } -#endif - mxArray* mxh = mxGetProperty(obj,0,"self"); - if (mxh==NULL) error("unwrap_reference: invalid wrap object"); - ObjectHandle* handle = ObjectHandle::from_mex_handle(mxh); - return handle->get_object(); -} - -// Diagonal -template <> -boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& className) { -#ifndef UNSAFE_WRAP - bool isDiagonal = mxIsClass(obj, "gtsamnoiseModelDiagonal"); - bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic"); - bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit"); - if (!isDiagonal && !isIsotropic && !isUnit ) { - mexPrintf("Expected gtsamnoiseModelDiagonal or derived classes, got %s\n", mxGetClassName(obj)); - error("Argument has wrong type."); - } -#endif - mxArray* mxh = mxGetProperty(obj,0,"self"); - if (mxh==NULL) error("unwrap_reference: invalid wrap object"); - ObjectHandle* handle = ObjectHandle::from_mex_handle(mxh); - return handle->get_object(); -} - -// Gaussian -template <> -boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& className) { -#ifndef UNSAFE_WRAP - bool isGaussian = mxIsClass(obj, "gtsamnoiseModelGaussian"); - bool isDiagonal = mxIsClass(obj, "gtsamnoiseModelDiagonal"); - bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic"); - bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit"); - if (!isGaussian && !isDiagonal && !isIsotropic && !isUnit) { - mexPrintf("Expected gtsamnoiseModelGaussian or derived classes, got %s\n", mxGetClassName(obj)); - error("Argument has wrong type."); - } -#endif - mxArray* mxh = mxGetProperty(obj,0,"self"); - if (mxh==NULL) error("unwrap_reference: invalid wrap object"); - ObjectHandle* handle = ObjectHandle::from_mex_handle(mxh); - return handle->get_object(); -} - -// Base -template <> -boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& className) { -#ifndef UNSAFE_WRAP - bool isBase = mxIsClass(obj, "gtsamnoiseModelBase"); - bool isGaussian = mxIsClass(obj, "gtsamnoiseModelGaussian"); - bool isDiagonal = mxIsClass(obj, "gtsamnoiseModelDiagonal"); - bool isIsotropic = mxIsClass(obj, "gtsamnoiseModelIsotropic"); - bool isUnit = mxIsClass(obj, "gtsamnoiseModelUnit"); - if (!isBase && !isGaussian && !isDiagonal && !isIsotropic && !isUnit) { - mexPrintf("Expected gtsamnoiseModelBase or derived classes, got %s\n", mxGetClassName(obj)); - error("Argument has wrong type."); - } -#endif - mxArray* mxh = mxGetProperty(obj,0,"self"); - if (mxh==NULL) error("unwrap_reference: invalid wrap object"); - ObjectHandle* handle = ObjectHandle::from_mex_handle(mxh); - return handle->get_object(); -} - -//end specialized templates template void delete_shared_ptr(const mxArray* obj, const string& className) { - //Why is this here? -#ifndef UNSAFE_WRAP - bool isClass = true;//mxIsClass(obj, className.c_str()); - if (!isClass) { - mexPrintf("Expected %s, got %s\n", className.c_str(), mxGetClassName(obj)); - error("Argument has wrong type."); - } -#endif mxArray* mxh = mxGetProperty(obj,0,"self"); - if (mxh==NULL) error("unwrap_reference: invalid wrap object"); ObjectHandle* handle = ObjectHandle::from_mex_handle(mxh); delete handle; } - -//***************************************************************************** diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index bfd1e77df..00606f930 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -12,6 +12,7 @@ /** * @file utilities.ccp * @author Frank Dellaert + * @author Andrew Melim **/ #include @@ -103,10 +104,12 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) } /* ************************************************************************* */ -string maybe_shared_ptr(bool add, const string& type) { - string str = add? "boost::shared_ptr<" : ""; - str += type; - if (add) str += ">"; +string maybe_shared_ptr(bool add, const string& qtype, const string& type) { + string str = add? "Shared" : ""; + if (add) str += type; + else str += qtype; + + //if (add) str += ">"; return str; } @@ -121,6 +124,7 @@ void generateUsingNamespace(FileWriter& file, const vector& using_namesp void generateIncludes(FileWriter& file, const string& class_name, const vector& includes) { file.oss << "#include " << endl; + file.oss << "#include " << endl; bool added_include = false; BOOST_FOREACH(const string& s, includes) { if (!s.empty()) { @@ -134,4 +138,5 @@ void generateIncludes(FileWriter& file, const string& class_name, /* ************************************************************************* */ + } // \namespace wrap diff --git a/wrap/utilities.h b/wrap/utilities.h index 384ff3749..1eb0e249f 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -10,8 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file utilities.ccp + * @file utilities.h * @author Frank Dellaert + * @author Andrew Melim **/ #pragma once @@ -84,7 +85,7 @@ bool assert_equal(const std::string& expected, const std::string& actual); bool assert_equal(const std::vector& expected, const std::vector& actual); // auxiliary function to wrap an argument into a shared_ptr template -std::string maybe_shared_ptr(bool add, const std::string& type); +std::string maybe_shared_ptr(bool add, const std::string& qtype, const std::string& type); /** * Creates the "using namespace [name];" declarations From 642515a322c6ae81564afbb6d658b99258c61e73 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Tue, 26 Jun 2012 20:40:15 +0000 Subject: [PATCH 03/21] All matlab tests complete. --- gtsam.h | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/gtsam.h b/gtsam.h index 941e39af7..e4993413a 100644 --- a/gtsam.h +++ b/gtsam.h @@ -921,6 +921,11 @@ class NonlinearFactor { // size_t dim() const; // FIXME: Doesn't link }; +class EasyFactorGraph { + EasyFactorGraph(); + void print(string s) const; +}; + class Values { Values(); size_t size() const; @@ -937,7 +942,7 @@ class KeyList { // Note: no print function size_t size() const; }; - +// FIXME // Actually a KeyVector #include class KeyVector { @@ -959,9 +964,9 @@ class Marginals { #include class LevenbergMarquardtParams { - /*LevenbergMarquardtParams(); + LevenbergMarquardtParams(); LevenbergMarquardtParams(double initial, double factor, double bound, size_t verbose); - void print(string s) const;*/ + void print(string s) const; double getlambdaInitial() const ; double getlambdaFactor() const ; double getlambdaUpperBound() const; @@ -1075,7 +1080,7 @@ class Graph { void addPosePrior(size_t key, const gtsam::Pose3& p, const gtsam::noiseModel::Base* model); void addRelativePose(size_t key1, size_t key2, const gtsam::Pose3& z, const gtsam::noiseModel::Base* model); pose3SLAM::Values optimize(const pose3SLAM::Values& initialEstimate, size_t verbosity) const; - // FIXME gtsam::LevenbergMarquardtOptimizer optimizer(const pose3SLAM::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const; + //gtsam::LevenbergMarquardtOptimizer optimizer(const pose3SLAM::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const; gtsam::Marginals marginals(const pose3SLAM::Values& solution) const; }; From ea0c85ef069c2d29e4c377c80b38b18eafb0938a Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Wed, 27 Jun 2012 16:22:12 +0000 Subject: [PATCH 04/21] Crash for some objects is now fixed --- wrap/Constructor.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 0ee459389..97a6af4fc 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -107,14 +107,13 @@ void Constructor::matlab_wrapper(const string& toolboxPath, file.oss << "{" << endl; file.oss << " void operator() (Shared* p)" << endl; file.oss << " {" << endl; - file.oss << " delete p;" << endl; + file.oss << " collector.erase(p);" << endl; file.oss << " }" << endl; file.oss << "};" << endl; file.oss << endl; //Generate cleanup function file.oss << "void cleanup(void) {" << endl; - file.oss << " mexPrintf(\"MEX-file new_gtsamDummy_ is terminating\");" << endl; file.oss << " std::for_each( collector.begin(), collector.end(), Destruct() );" << endl; file.oss << "}" << endl; @@ -129,13 +128,13 @@ void Constructor::matlab_wrapper(const string& toolboxPath, file.oss << " if(self) {" << endl; file.oss << " if(nargin > 1) {" << endl; file.oss << " collector.insert(self);" << endl; + //TODO: Add verbosity flag file.oss << " std::cout << \"Collected\" << collector.size() << std::endl;" << endl; file.oss << " }" << endl; file.oss << " else if(collector.erase(self))" << endl; file.oss << " delete self;" << endl; file.oss << " } else {" << endl; file.oss << " int nc = unwrap(in[1]);" << endl; - file.oss << " mexPrintf(\"MEX-file constructor\");" << endl << endl;; int i = 0; BOOST_FOREACH(ArgumentList al, args_list) From ddbea256af1f5e1dc627c699df6a1e0289893de7 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 27 Jun 2012 20:03:13 +0000 Subject: [PATCH 05/21] Merge branch 'master' into new_wrap_local --- gtsam/discrete/DiscreteConditional.h | 6 +- gtsam/discrete/DiscreteFactor.h | 6 +- gtsam/discrete/DiscreteFactorGraph.cpp | 13 ++ gtsam/discrete/DiscreteFactorGraph.h | 11 +- gtsam/inference/BayesNet-inl.h | 23 +- gtsam/inference/BayesNet.h | 8 +- gtsam/inference/BayesTree-inl.h | 12 + gtsam/inference/BayesTree.h | 1 + gtsam/inference/BayesTreeCliqueBase.h | 3 + gtsam/inference/ClusterTree-inl.h | 17 +- gtsam/inference/ClusterTree.h | 10 +- gtsam/inference/EliminationTree-inl.h | 9 +- gtsam/inference/EliminationTree.h | 7 +- gtsam/inference/Factor-inl.h | 5 +- gtsam/inference/Factor.h | 5 +- gtsam/inference/FactorGraph-inl.h | 5 +- gtsam/inference/FactorGraph.h | 3 +- gtsam/linear/GaussianDensity.cpp | 4 +- gtsam/linear/GaussianDensity.h | 3 +- gtsam/linear/VectorValues.cpp | 4 +- gtsam/linear/VectorValues.h | 10 +- gtsam/nonlinear/NonlinearISAM.cpp | 8 +- tests/testSubgraphPreconditioner.cpp | 297 ++++++++++++------------- 23 files changed, 266 insertions(+), 204 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index c95cc36d6..6de9989f2 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -63,9 +63,11 @@ namespace gtsam { /// @{ /** GTSAM-style print */ - void print(const std::string& s = "Discrete Conditional: ") const { + void print(const std::string& s = "Discrete Conditional: ", + const boost::function& formatter + = &(boost::lexical_cast)) const { std::cout << s << std::endl; - IndexConditional::print(s); + IndexConditional::print(s, formatter); Potentials::print(s); } diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 0bfdb3068..deac7efce 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -82,8 +82,10 @@ namespace gtsam { /// @{ // print - virtual void print(const std::string& s = "DiscreteFactor\n") const { - IndexFactor::print(s); + virtual void print(const std::string& s = "DiscreteFactor\n", + const boost::function& formatter + = &(boost::lexical_cast)) const { + IndexFactor::print(s,formatter); } /// @} diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 4726c752f..f3c89721b 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -63,6 +63,18 @@ namespace gtsam { return product; } + /* ************************************************************************* */ + void DiscreteFactorGraph::print(const std::string& s, + const IndexFormatter& formatter) const { + std::cout << s << std::endl; + std::cout << "size: " << size() << std::endl; + for (size_t i = 0; i < factors_.size(); i++) { + std::stringstream ss; + ss << "factor " << i << ": "; + if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); + } + } + /* ************************************************************************* */ std::pair // EliminateDiscrete(const FactorGraph& factors, size_t num) { @@ -90,6 +102,7 @@ namespace gtsam { return std::make_pair(cond, sum); } + /* ************************************************************************* */ } // namespace diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index d96a3049a..44b814507 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -77,15 +77,8 @@ public: double operator()(const DiscreteFactor::Values & values) const; /// print - void print(const std::string& s = "DiscreteFactorGraph") const { - std::cout << s << std::endl; - std::cout << "size: " << size() << std::endl; - for (size_t i = 0; i < factors_.size(); i++) { - std::stringstream ss; - ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str()); - } - } + void print(const std::string& s = "DiscreteFactorGraph", + const IndexFormatter& formatter = &(boost::lexical_cast)) const; }; // DiscreteFactorGraph diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index 57cef4634..89361e8fc 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -26,6 +26,7 @@ #include // for += using boost::assign::operator+=; +#include #include #include @@ -33,12 +34,30 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesNet::print(const std::string& s) const { + void BayesNet::print(const std::string& s, + const boost::function& formatter) const { std::cout << s; BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) - conditional->print(); + conditional->print("Conditional", formatter); } + /* ************************************************************************* */ + template + void BayesNet::printStats(const std::string& s) const { + + const size_t n = conditionals_.size(); + size_t max_size = 0; + size_t total = 0; + BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) { + max_size = std::max(max_size, conditional->size()); + total += conditional->size(); + } + std::cout << s + << "maximum conditional size = " << max_size << std::endl + << "average conditional size = " << total / n << std::endl + << "density = " << 100.0 * total / (double) (n*(n+1)/2) << " %" << std::endl; + } + /* ************************************************************************* */ template bool BayesNet::equals(const BayesNet& cbn, double tol) const { diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index f6721816c..32abd9ed6 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -48,6 +49,7 @@ public: typedef typename boost::shared_ptr > shared_ptr; /** We store shared pointers to Conditional densities */ + typedef typename CONDITIONAL::KeyType KeyType; typedef typename boost::shared_ptr sharedConditional; typedef typename boost::shared_ptr const_sharedConditional; typedef typename std::list Conditionals; @@ -88,7 +90,11 @@ public: /// @{ /** print */ - void print(const std::string& s = "") const; + void print(const std::string& s = "", + const boost::function& formatter = &(boost::lexical_cast)) const; + + /** print statistics */ + void printStats(const std::string& s = "") const; /** check equality */ bool equals(const BayesNet& other, double tol = 1e-9) const; diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 22dd0c62b..501a8aabd 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -67,6 +67,7 @@ namespace gtsam { of.close(); } + /* ************************************************************************* */ template void BayesTree::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const { static int num = 0; @@ -102,6 +103,17 @@ namespace gtsam { } + /* ************************************************************************* */ + template + void BayesTree::CliqueStats::print(const std::string& s) const { + std::cout << s + <<"avg Conditional Size: " << avgConditionalSize << std::endl + << "max Conditional Size: " << maxConditionalSize << std::endl + << "avg Separator Size: " << avgSeparatorSize << std::endl + << "max Separator Size: " << maxSeparatorSize << std::endl; + } + + /* ************************************************************************* */ template typename BayesTree::CliqueStats BayesTree::CliqueData::getStats() const { diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 9c38d6496..6413e4b0d 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -81,6 +81,7 @@ namespace gtsam { std::size_t maxConditionalSize; double avgSeparatorSize; std::size_t maxSeparatorSize; + void print(const std::string& s = "") const ; }; /** store all the sizes */ diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 9a2f6c54e..d054fe43f 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -117,6 +117,9 @@ namespace gtsam { /** return the const reference of children */ const std::list& children() const { return children_; } + /** return a shared_ptr to the parent clique */ + derived_ptr parent() const { return parent_.lock(); } + /// @} /// @name Advanced Interface /// @{ diff --git a/gtsam/inference/ClusterTree-inl.h b/gtsam/inference/ClusterTree-inl.h index 548a55cf4..b3fe204e7 100644 --- a/gtsam/inference/ClusterTree-inl.h +++ b/gtsam/inference/ClusterTree-inl.h @@ -71,10 +71,11 @@ namespace gtsam { /* ************************************************************************* */ template - void ClusterTree::Cluster::print(const std::string& indent) const { + void ClusterTree::Cluster::print(const std::string& indent, + const boost::function& formatter) const { std::cout << indent; BOOST_FOREACH(const Index key, frontal) - std::cout << key << " "; + std::cout << formatter(key) << " "; std::cout << ": "; BOOST_FOREACH(const Index key, separator) std::cout << key << " "; @@ -83,19 +84,21 @@ namespace gtsam { /* ************************************************************************* */ template - void ClusterTree::Cluster::printTree(const std::string& indent) const { - print(indent); + void ClusterTree::Cluster::printTree(const std::string& indent, + const boost::function& formatter) const { + print(indent, formatter); BOOST_FOREACH(const shared_ptr& child, children_) - child->printTree(indent + " "); + child->printTree(indent + " ", formatter); } /* ************************************************************************* * * ClusterTree * ************************************************************************* */ template - void ClusterTree::print(const std::string& str) const { + void ClusterTree::print(const std::string& str, + const boost::function& formatter) const { std::cout << str << std::endl; - if (root_) root_->printTree(""); + if (root_) root_->printTree("", formatter); } /* ************************************************************************* */ diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 92919b936..f253fe4d6 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -25,6 +25,7 @@ #include #include +#include #include @@ -38,6 +39,9 @@ namespace gtsam { */ template class ClusterTree { + public: + // Access to factor types + typedef typename FG::KeyType KeyType; protected: @@ -74,10 +78,10 @@ namespace gtsam { Cluster(FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator); /// print - void print(const std::string& indent) const; + void print(const std::string& indent, const boost::function& formatter = &(boost::lexical_cast)) const; /// print the enire tree - void printTree(const std::string& indent) const; + void printTree(const std::string& indent, const boost::function& formatter = &(boost::lexical_cast)) const; /// check equality bool equals(const Cluster& other) const; @@ -123,7 +127,7 @@ namespace gtsam { /// @{ /// print the object - void print(const std::string& str="") const; + void print(const std::string& str="", const boost::function& formatter = &(boost::lexical_cast)) const; /** check equality */ bool equals(const ClusterTree& other, double tol = 1e-9) const; diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index c4cb9d4cf..81c98c926 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -166,12 +166,13 @@ EliminationTree::Create(const FactorGraph& factorGraph) { /* ************************************************************************* */ template -void EliminationTree::print(const std::string& name) const { - std::cout << name << " (" << key_ << ")" << std::endl; +void EliminationTree::print(const std::string& name, + const boost::function& formatter) const { + std::cout << name << " (" << formatter(key_) << ")" << std::endl; BOOST_FOREACH(const sharedFactor& factor, factors_) { - factor->print(name + " "); } + factor->print(name + " ", formatter); } BOOST_FOREACH(const shared_ptr& child, subTrees_) { - child->print(name + " "); } + child->print(name + " ", formatter); } } /* ************************************************************************* */ diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index 6ab7dcf49..8e57a7208 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -54,6 +54,8 @@ public: typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class typedef typename boost::shared_ptr sharedFactor; ///< Shared pointer to a factor typedef gtsam::BayesNet BayesNet; ///< The BayesNet corresponding to FACTOR + typedef FACTOR Factor; + typedef typename FACTOR::KeyType KeyType; /** Typedef for an eliminate subroutine */ typedef typename FactorGraph::Eliminate Eliminate; @@ -67,7 +69,7 @@ private: typedef FastList SubTrees; typedef std::vector Conditionals; - Index key_; ///< index associated with root + Index key_; ///< index associated with root // FIXME: doesn't this require that "Index" is the type of keys in the generic factor? Factors factors_; ///< factors associated with root SubTrees subTrees_; ///< sub-trees @@ -141,7 +143,8 @@ public: /// @{ /** Print the tree to cout */ - void print(const std::string& name = "EliminationTree: ") const; + void print(const std::string& name = "EliminationTree: ", + const boost::function& formatter = &(boost::lexical_cast)) const; /** Test whether the tree is equal to another */ bool equals(const EliminationTree& other, double tol = 1e-9) const; diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h index d9fb1e0de..ad9f43701 100644 --- a/gtsam/inference/Factor-inl.h +++ b/gtsam/inference/Factor-inl.h @@ -56,9 +56,10 @@ namespace gtsam { /* ************************************************************************* */ template - void Factor::print(const std::string& s) const { + void Factor::print(const std::string& s, + const boost::function& formatter) const { std::cout << s << " "; - BOOST_FOREACH(KEY key, keys_) std::cout << " " << key; + BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key); std::cout << std::endl; } diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 6ed9c809b..0e80fc8ea 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include namespace gtsam { @@ -190,7 +192,8 @@ public: /// @{ /// print - void print(const std::string& s = "Factor") const; + void print(const std::string& s = "Factor", + const boost::function& formatter = &(boost::lexical_cast)) const; /// check equality bool equals(const This& other, double tol = 1e-9) const; diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 90212f39f..c787655af 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -48,13 +48,14 @@ namespace gtsam { /* ************************************************************************* */ template - void FactorGraph::print(const std::string& s) const { + void FactorGraph::print(const std::string& s, + const boost::function& formatter) const { std::cout << s << std::endl; std::cout << "size: " << size() << std::endl; for (size_t i = 0; i < factors_.size(); i++) { std::stringstream ss; ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str()); + if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); } } diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 89662ffc4..905afc55c 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -138,7 +138,8 @@ template class BayesTree; /// @{ /** print out graph */ - void print(const std::string& s = "FactorGraph") const; + void print(const std::string& s = "FactorGraph", + const boost::function& formatter = &(boost::lexical_cast)) const; /** Check equality */ bool equals(const This& fg, double tol = 1e-9) const; diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index 97e42d9ba..c8fc4c21a 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -23,11 +23,11 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - void GaussianDensity::print(const string &s) const + void GaussianDensity::print(const string &s, const IndexFormatter& formatter) const { cout << s << ": density on "; for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) - cout << (boost::format("[%1%]")%(*it)).str() << " "; + cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; cout << endl; gtsam::print(Matrix(get_R()),"R"); gtsam::print(Vector(get_d()),"d"); diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 2c13e847c..61db582f4 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -54,7 +54,8 @@ namespace gtsam { } /// print - void print(const std::string& = "GaussianDensity") const; + void print(const std::string& = "GaussianDensity", + const IndexFormatter& formatter = &(boost::lexical_cast)) const; /// Mean \f$ \mu = R^{-1} d \f$ Vector mean() const; diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 5868d8452..1413d1517 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -77,10 +77,10 @@ void VectorValues::insert(Index j, const Vector& value) { } /* ************************************************************************* */ -void VectorValues::print(const std::string& str) const { +void VectorValues::print(const std::string& str, const IndexFormatter& formatter) const { std::cout << str << ": " << size() << " elements\n"; for (Index var = 0; var < size(); ++var) - std::cout << " " << var << ": \n" << operator[](var) << "\n"; + std::cout << " " << formatter(var) << ": \n" << operator[](var) << "\n"; std::cout.flush(); } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index f3d82511e..89c59a6ad 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -159,15 +160,16 @@ namespace gtsam { iterator begin() { chk(); return maps_.begin(); } ///< Iterator over variables const_iterator begin() const { chk(); return maps_.begin(); } ///< Iterator over variables - iterator end() { chk(); return maps_.end(); } ///< Iterator over variables + iterator end() { chk(); return maps_.end(); } ///< Iterator over variables const_iterator end() const { chk(); return maps_.end(); } ///< Iterator over variables - reverse_iterator rbegin() { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables + reverse_iterator rbegin() { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables const_reverse_iterator rbegin() const { chk(); return maps_.rbegin(); } ///< Reverse iterator over variables - reverse_iterator rend() { chk(); return maps_.rend(); } ///< Reverse iterator over variables + reverse_iterator rend() { chk(); return maps_.rend(); } ///< Reverse iterator over variables const_reverse_iterator rend() const { chk(); return maps_.rend(); } ///< Reverse iterator over variables /** print required by Testable for unit testing */ - void print(const std::string& str = "VectorValues: ") const; + void print(const std::string& str = "VectorValues: ", + const IndexFormatter& formatter = &(boost::lexical_cast)) const; /** equals required by Testable for unit testing */ bool equals(const VectorValues& x, double tol = 1e-9) const; diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 6b5ea31bc..b2135fedd 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -118,13 +118,7 @@ void NonlinearISAM::print(const string& s, const KeyFormatter& keyFormatter) con /* ************************************************************************* */ void NonlinearISAM::printStats() const { - gtsam::GaussianISAM::CliqueData data = isam_.getCliqueData(); - gtsam::GaussianISAM::CliqueStats stats = data.getStats(); - cout << "\navg Conditional Size: " << stats.avgConditionalSize; - cout << "\nmax Conditional Size: " << stats.maxConditionalSize; - cout << "\navg Separator Size: " << stats.avgSeparatorSize; - cout << "\nmax Separator Size: " << stats.maxSeparatorSize; - cout << endl; + isam_.getCliqueData().getStats().print(); } /* ************************************************************************* */ diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index 382ac4cc7..417643e78 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -40,14 +40,14 @@ Key i3002 = 3002, i2002 = 2002, i1002 = 1002; Key i3001 = 3001, i2001 = 2001, i1001 = 1001; // TODO fix Ordering::equals, because the ordering *is* correct ! -/* ************************************************************************* * -TEST( SubgraphPreconditioner, planarOrdering ) -{ - // Check canonical ordering - Ordering expected, ordering = planarOrdering(3); - expected += i3003, i2003, i1003, i3002, i2002, i1002, i3001, i2001, i1001; - CHECK(assert_equal(expected,ordering)); -} +/* ************************************************************************* */ +//TEST( SubgraphPreconditioner, planarOrdering ) +//{ +// // Check canonical ordering +// Ordering expected, ordering = planarOrdering(3); +// expected += i3003, i2003, i1003, i3002, i2002, i1002, i3001, i2001, i1001; +// CHECK(assert_equal(expected,ordering)); +//} /* ************************************************************************* */ TEST( SubgraphPreconditioner, planarGraph ) @@ -66,148 +66,145 @@ TEST( SubgraphPreconditioner, planarGraph ) CHECK(assert_equal(xtrue,actual)); } -/* ************************************************************************* * -TEST( SubgraphPreconditioner, splitOffPlanarTree ) -{ - // Build a planar graph - GaussianFactorGraph A; - VectorValues xtrue; - boost::tie(A, xtrue) = planarGraph(3); - - // Get the spanning tree and constraints, and check their sizes - JacobianFactorGraph T, C; - // TODO big mess: GFG and JFG mess !!! - boost::tie(T, C) = splitOffPlanarTree(3, A); - LONGS_EQUAL(9,T.size()); - LONGS_EQUAL(4,C.size()); - - // Check that the tree can be solved to give the ground xtrue - GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); - VectorValues xbar = optimize(*R1); - CHECK(assert_equal(xtrue,xbar)); -} - -/* ************************************************************************* * -TEST( SubgraphPreconditioner, system ) -{ - // Build a planar graph - JacobianFactorGraph Ab; - VectorValues xtrue; - size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b - - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); - - // Eliminate the spanning tree to build a prior - SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 - VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 - - // Create Subgraph-preconditioned system - VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible - SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); - - // Create zero config - VectorValues zeros = VectorValues::Zero(xbar); - - // Set up y0 as all zeros - VectorValues y0 = zeros; - - // y1 = perturbed y0 - VectorValues y1 = zeros; - y1[i2003] = Vector_(2, 1.0, -1.0); - - // Check corresponding x values - VectorValues expected_x1 = xtrue, x1 = system.x(y1); - expected_x1[i2003] = Vector_(2, 2.01, 2.99); - expected_x1[i3003] = Vector_(2, 3.01, 2.99); - CHECK(assert_equal(xtrue, system.x(y0))); - CHECK(assert_equal(expected_x1,system.x(y1))); - - // Check errors -// DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9); // TODO ! -// DOUBLES_EQUAL(3,error(Ab,x1),1e-9); // TODO ! - DOUBLES_EQUAL(0,error(system,y0),1e-9); - DOUBLES_EQUAL(3,error(system,y1),1e-9); - - // Test gradient in x - VectorValues expected_gx0 = zeros; - VectorValues expected_gx1 = zeros; - CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); - expected_gx1[i1003] = Vector_(2, -100., 100.); - expected_gx1[i2002] = Vector_(2, -100., 100.); - expected_gx1[i2003] = Vector_(2, 200., -200.); - expected_gx1[i3002] = Vector_(2, -100., 100.); - expected_gx1[i3003] = Vector_(2, 100., -100.); - CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); - - // Test gradient in y - VectorValues expected_gy0 = zeros; - VectorValues expected_gy1 = zeros; - expected_gy1[i1003] = Vector_(2, 2., -2.); - expected_gy1[i2002] = Vector_(2, -2., 2.); - expected_gy1[i2003] = Vector_(2, 3., -3.); - expected_gy1[i3002] = Vector_(2, -1., 1.); - expected_gy1[i3003] = Vector_(2, 1., -1.); - CHECK(assert_equal(expected_gy0,gradient(system,y0))); - CHECK(assert_equal(expected_gy1,gradient(system,y1))); - - // Check it numerically for good measure - // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) - // Vector numerical_g1 = numericalGradient (error, y1, 0.001); - // Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., - // 3., -3., 0., 0., -1., 1., 1., -1.); - // CHECK(assert_equal(expected_g1,numerical_g1)); -} - -/* ************************************************************************* * -TEST( SubgraphPreconditioner, conjugateGradients ) -{ - // Build a planar graph - GaussianFactorGraph Ab; - VectorValues xtrue; - size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b - - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); - - // Eliminate the spanning tree to build a prior - Ordering ordering = planarOrdering(N); - SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 - VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 - - // Create Subgraph-preconditioned system - VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible - SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); - - // Create zero config y0 and perturbed config y1 - VectorValues y0 = VectorValues::Zero(xbar); - - VectorValues y1 = y0; - y1[i2003] = Vector_(2, 1.0, -1.0); - VectorValues x1 = system.x(y1); - - // Solve for the remaining constraints using PCG - ConjugateGradientParameters parameters; -// VectorValues actual = gtsam::conjugateGradients(system, y1, verbose, epsilon, epsilon, maxIterations); -// CHECK(assert_equal(y0,actual)); - - // Compare with non preconditioned version: - VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); - CHECK(assert_equal(xtrue,actual2,1e-4)); -} +/* ************************************************************************* */ +//TEST( SubgraphPreconditioner, splitOffPlanarTree ) +//{ +// // Build a planar graph +// GaussianFactorGraph A; +// VectorValues xtrue; +// boost::tie(A, xtrue) = planarGraph(3); +// +// // Get the spanning tree and constraints, and check their sizes +// JacobianFactorGraph T, C; +// // TODO big mess: GFG and JFG mess !!! +// boost::tie(T, C) = splitOffPlanarTree(3, A); +// LONGS_EQUAL(9,T.size()); +// LONGS_EQUAL(4,C.size()); +// +// // Check that the tree can be solved to give the ground xtrue +// GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); +// VectorValues xbar = optimize(*R1); +// CHECK(assert_equal(xtrue,xbar)); +//} /* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} +//TEST( SubgraphPreconditioner, system ) +//{ +// // Build a planar graph +// JacobianFactorGraph Ab; +// VectorValues xtrue; +// size_t N = 3; +// boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b +// +// // Get the spanning tree and corresponding ordering +// GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 +// boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); +// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); +// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); +// +// // Eliminate the spanning tree to build a prior +// SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 +// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 +// +// // Create Subgraph-preconditioned system +// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible +// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); +// +// // Create zero config +// VectorValues zeros = VectorValues::Zero(xbar); +// +// // Set up y0 as all zeros +// VectorValues y0 = zeros; +// +// // y1 = perturbed y0 +// VectorValues y1 = zeros; +// y1[i2003] = Vector_(2, 1.0, -1.0); +// +// // Check corresponding x values +// VectorValues expected_x1 = xtrue, x1 = system.x(y1); +// expected_x1[i2003] = Vector_(2, 2.01, 2.99); +// expected_x1[i3003] = Vector_(2, 3.01, 2.99); +// CHECK(assert_equal(xtrue, system.x(y0))); +// CHECK(assert_equal(expected_x1,system.x(y1))); +// +// // Check errors +//// DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9); // TODO ! +//// DOUBLES_EQUAL(3,error(Ab,x1),1e-9); // TODO ! +// DOUBLES_EQUAL(0,error(system,y0),1e-9); +// DOUBLES_EQUAL(3,error(system,y1),1e-9); +// +// // Test gradient in x +// VectorValues expected_gx0 = zeros; +// VectorValues expected_gx1 = zeros; +// CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); +// expected_gx1[i1003] = Vector_(2, -100., 100.); +// expected_gx1[i2002] = Vector_(2, -100., 100.); +// expected_gx1[i2003] = Vector_(2, 200., -200.); +// expected_gx1[i3002] = Vector_(2, -100., 100.); +// expected_gx1[i3003] = Vector_(2, 100., -100.); +// CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); +// +// // Test gradient in y +// VectorValues expected_gy0 = zeros; +// VectorValues expected_gy1 = zeros; +// expected_gy1[i1003] = Vector_(2, 2., -2.); +// expected_gy1[i2002] = Vector_(2, -2., 2.); +// expected_gy1[i2003] = Vector_(2, 3., -3.); +// expected_gy1[i3002] = Vector_(2, -1., 1.); +// expected_gy1[i3003] = Vector_(2, 1., -1.); +// CHECK(assert_equal(expected_gy0,gradient(system,y0))); +// CHECK(assert_equal(expected_gy1,gradient(system,y1))); +// +// // Check it numerically for good measure +// // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) +// // Vector numerical_g1 = numericalGradient (error, y1, 0.001); +// // Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., +// // 3., -3., 0., 0., -1., 1., 1., -1.); +// // CHECK(assert_equal(expected_g1,numerical_g1)); +//} + +/* ************************************************************************* */ +//TEST( SubgraphPreconditioner, conjugateGradients ) +//{ +// // Build a planar graph +// GaussianFactorGraph Ab; +// VectorValues xtrue; +// size_t N = 3; +// boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b +// +// // Get the spanning tree and corresponding ordering +// GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 +// boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); +// SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); +// SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); +// +// // Eliminate the spanning tree to build a prior +// Ordering ordering = planarOrdering(N); +// SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 +// VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 +// +// // Create Subgraph-preconditioned system +// VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible +// SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared); +// +// // Create zero config y0 and perturbed config y1 +// VectorValues y0 = VectorValues::Zero(xbar); +// +// VectorValues y1 = y0; +// y1[i2003] = Vector_(2, 1.0, -1.0); +// VectorValues x1 = system.x(y1); +// +// // Solve for the remaining constraints using PCG +// ConjugateGradientParameters parameters; +//// VectorValues actual = gtsam::conjugateGradients(system, y1, verbose, epsilon, epsilon, maxIterations); +//// CHECK(assert_equal(y0,actual)); +// +// // Compare with non preconditioned version: +// VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); +// CHECK(assert_equal(xtrue,actual2,1e-4)); +//} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From fb00f4b834ac3956dc4ce99e29dbace18999b96f Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Wed, 27 Jun 2012 21:50:45 +0000 Subject: [PATCH 06/21] fixes for two word args --- wrap/Class.cpp | 3 ++- wrap/Constructor.cpp | 2 +- wrap/Method.cpp | 9 ++++++--- wrap/matlab.h | 6 ------ 4 files changed, 9 insertions(+), 11 deletions(-) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 7280d5488..5e0237d3a 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -59,7 +59,8 @@ void Class::matlab_proxy(const string& classFile) const { // deconstructor file.oss << " function delete(obj)" << endl; file.oss << " if obj.self ~= 0" << endl; - file.oss << " fprintf(1,'MATLAB class deleting %x',obj.self);" << endl; + //TODO: Add verbosity flag + //file.oss << " fprintf(1,'MATLAB class deleting %x',obj.self);" << endl; file.oss << " new_" << matlabName << "_(obj.self);" << endl; file.oss << " obj.self = 0;" << endl; file.oss << " end" << endl; diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 97a6af4fc..e999951b2 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -129,7 +129,7 @@ void Constructor::matlab_wrapper(const string& toolboxPath, file.oss << " if(nargin > 1) {" << endl; file.oss << " collector.insert(self);" << endl; //TODO: Add verbosity flag - file.oss << " std::cout << \"Collected\" << collector.size() << std::endl;" << endl; + //file.oss << " std::cout << \"Collected\" << collector.size() << std::endl;" << endl; file.oss << " }" << endl; file.oss << " else if(collector.erase(self))" << endl; file.oss << " delete self;" << endl; diff --git a/wrap/Method.cpp b/wrap/Method.cpp index ccd36fe50..f5da0d370 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -63,11 +63,14 @@ void Method::matlab_wrapper(const string& classPath, if(returnVal.isPair) { - file.oss << "typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; - file.oss << "typedef boost::shared_ptr<" << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 << ";"<< endl; + 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 - file.oss << "typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl; + 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; // call diff --git a/wrap/matlab.h b/wrap/matlab.h index 99dd0c6b3..74c0823e4 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -341,7 +341,6 @@ mxArray* create_collect_object(const char *classname, mxArray* h){ mxArray* dummy[14] = {h,h,h,h,h, h,h,h,h,h, h,h,h,h}; mexCallMATLAB(1,&result,14,dummy,classname); mxSetProperty(result, 0, "self", h); - cout << "Return collect" << endl; return result; } @@ -354,7 +353,6 @@ template mxArray* wrap_shared_ptr(boost::shared_ptr< Class >* shared_ptr, const char *classname) { mxArray* mxh = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast**> (mxGetPr(mxh)) = shared_ptr; - cout << "wrapped:" << mxh << endl << "end wrap" << endl; //return mxh; return create_object(classname, mxh); } @@ -363,23 +361,19 @@ template mxArray* wrap_collect_shared_ptr(boost::shared_ptr< Class >* shared_ptr, const char *classname) { mxArray* mxh = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast**> (mxGetPr(mxh)) = shared_ptr; - cout << "wrapped:" << mxh << endl << "end wrap" << endl; //return mxh; return create_collect_object(classname, mxh); } template boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& className) { - cout << "UNWRAP CALL" << endl; mxArray* mxh = mxGetProperty(obj,0,"self"); if (mxGetClassID(mxh) != mxUINT32OR64_CLASS || mxIsComplex(mxh) || mxGetM(mxh) != 1 || mxGetN(mxh) != 1) error( "Parameter is not an Shared type."); - cout << "unwrapped:" << mxh << endl; boost::shared_ptr* spp = *reinterpret_cast**> (mxGetPr(mxh)); - cout << "unwrapped:" << spp << endl; return *spp; } From 4abefa3cbeba5f1d3c4bb1748a63e981e9353558 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 28 Jun 2012 16:12:57 +0000 Subject: [PATCH 07/21] Merge branch 'master' into new_wrap_local --- CMakeLists.txt | 2 ++ gtsam/base/types.cpp | 30 +++++++++++++++++++++++++++ gtsam/base/types.h | 6 ++++++ gtsam/discrete/DiscreteConditional.h | 4 ++-- gtsam/discrete/DiscreteFactor.h | 4 ++-- gtsam/discrete/DiscreteFactorGraph.h | 2 +- gtsam/inference/BayesNet-inl.h | 2 +- gtsam/inference/BayesNet.h | 2 +- gtsam/inference/BayesTree.h | 6 +++--- gtsam/inference/BayesTreeCliqueBase.h | 4 ++-- gtsam/inference/ClusterTree-inl.h | 6 +++--- gtsam/inference/ClusterTree.h | 6 +++--- gtsam/inference/Conditional.h | 5 ++--- gtsam/inference/EliminationTree-inl.h | 2 +- gtsam/inference/EliminationTree.h | 2 +- gtsam/inference/Factor-inl.h | 2 +- gtsam/inference/Factor.h | 3 ++- gtsam/inference/FactorGraph-inl.h | 2 +- gtsam/inference/FactorGraph.h | 2 +- gtsam/linear/GaussianConditional.h | 2 +- gtsam/linear/GaussianDensity.h | 2 +- gtsam/linear/GaussianFactor.h | 2 +- gtsam/linear/HessianFactor.h | 2 +- gtsam/linear/JacobianFactor.h | 2 +- gtsam/linear/VectorValues.h | 2 +- gtsam/nonlinear/ISAM2.h | 2 +- 26 files changed, 72 insertions(+), 34 deletions(-) create mode 100644 gtsam/base/types.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fb25d2dd8..96efa97dd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,6 +50,7 @@ else() option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON) endif() option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" ON) +option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" OFF) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF) if(MSVC) option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" OFF) @@ -170,6 +171,7 @@ print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ") print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ") +print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build-type in library name ") print_config_flag(${GTSAM_BUILD_CONVENIENCE_LIBRARIES} "Build Convenience Libraries ") if(GTSAM_UNSTABLE_AVAILABLE) print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp new file mode 100644 index 000000000..ea4db72c8 --- /dev/null +++ b/gtsam/base/types.cpp @@ -0,0 +1,30 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file types.h + * @brief Typedefs for easier changing of types + * @author Richard Roberts + * @date Aug 21, 2010 + * @addtogroup base + */ + +#include + +#include + +namespace gtsam { + + std::string _defaultIndexFormatter(Index j) { + return boost::lexical_cast(j); + } + +} \ No newline at end of file diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 5559518b3..a42c2acc5 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -21,6 +21,7 @@ #include +#include #include namespace gtsam { @@ -32,6 +33,11 @@ namespace gtsam { * to a nonlinear key and then to a Symbol. */ typedef boost::function IndexFormatter; + std::string _defaultIndexFormatter(Index j); + + /** The default IndexFormatter outputs the index */ + static const IndexFormatter DefaultIndexFormatter = &_defaultIndexFormatter; + /** * Helper class that uses templates to select between two types based on * whether TEST_TYPE is const or not. diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 6de9989f2..e05bfd669 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -64,8 +64,8 @@ namespace gtsam { /** GTSAM-style print */ void print(const std::string& s = "Discrete Conditional: ", - const boost::function& formatter - = &(boost::lexical_cast)) const { + const IndexFormatter& formatter + =DefaultIndexFormatter) const { std::cout << s << std::endl; IndexConditional::print(s, formatter); Potentials::print(s); diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index deac7efce..9b1130bae 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -83,8 +83,8 @@ namespace gtsam { // print virtual void print(const std::string& s = "DiscreteFactor\n", - const boost::function& formatter - = &(boost::lexical_cast)) const { + const IndexFormatter& formatter + =DefaultIndexFormatter) const { IndexFactor::print(s,formatter); } diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 44b814507..0f399abdd 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -78,7 +78,7 @@ public: /// print void print(const std::string& s = "DiscreteFactorGraph", - const IndexFormatter& formatter = &(boost::lexical_cast)) const; + const IndexFormatter& formatter =DefaultIndexFormatter) const; }; // DiscreteFactorGraph diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index 89361e8fc..c306cb39d 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -35,7 +35,7 @@ namespace gtsam { /* ************************************************************************* */ template void BayesNet::print(const std::string& s, - const boost::function& formatter) const { + const IndexFormatter& formatter) const { std::cout << s; BOOST_REVERSE_FOREACH(sharedConditional conditional, conditionals_) conditional->print("Conditional", formatter); diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 32abd9ed6..67d59c48d 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -91,7 +91,7 @@ public: /** print */ void print(const std::string& s = "", - const boost::function& formatter = &(boost::lexical_cast)) const; + const IndexFormatter& formatter = DefaultIndexFormatter) const; /** print statistics */ void printStats(const std::string& s = "") const; diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 6413e4b0d..7391e9a10 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -71,7 +71,7 @@ namespace gtsam { // A convenience class for a list of shared cliques struct Cliques : public std::list { void print(const std::string& s = "Cliques", - const IndexFormatter& indexFormatter = &(boost::lexical_cast)) const; + const IndexFormatter& indexFormatter = DefaultIndexFormatter) const; bool equals(const Cliques& other, double tol = 1e-9) const; }; @@ -178,7 +178,7 @@ namespace gtsam { /** print */ void print(const std::string& s = "", - const IndexFormatter& indexFormatter = &(boost::lexical_cast) ) const; + const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; /// @} /// @name Standard Interface @@ -236,7 +236,7 @@ namespace gtsam { */ /** saves the Tree to a text file in GraphViz format */ - void saveGraph(const std::string& s, const IndexFormatter& indexFormatter = &(boost::lexical_cast) ) const; + void saveGraph(const std::string& s, const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; /// @} /// @name Advanced Interface diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index d054fe43f..a2fb8feef 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -93,10 +93,10 @@ namespace gtsam { } /** print this node */ - void print(const std::string& s = "", const IndexFormatter& indexFormatter = &(boost::lexical_cast) ) const; + void print(const std::string& s = "", const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; /** print this node and entire subtree below it */ - void printTree(const std::string& indent="", const IndexFormatter& indexFormatter = &(boost::lexical_cast) ) const; + void printTree(const std::string& indent="", const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; /// @} /// @name Standard Interface diff --git a/gtsam/inference/ClusterTree-inl.h b/gtsam/inference/ClusterTree-inl.h index b3fe204e7..3b8deb316 100644 --- a/gtsam/inference/ClusterTree-inl.h +++ b/gtsam/inference/ClusterTree-inl.h @@ -72,7 +72,7 @@ namespace gtsam { /* ************************************************************************* */ template void ClusterTree::Cluster::print(const std::string& indent, - const boost::function& formatter) const { + const IndexFormatter& formatter) const { std::cout << indent; BOOST_FOREACH(const Index key, frontal) std::cout << formatter(key) << " "; @@ -85,7 +85,7 @@ namespace gtsam { /* ************************************************************************* */ template void ClusterTree::Cluster::printTree(const std::string& indent, - const boost::function& formatter) const { + const IndexFormatter& formatter) const { print(indent, formatter); BOOST_FOREACH(const shared_ptr& child, children_) child->printTree(indent + " ", formatter); @@ -96,7 +96,7 @@ namespace gtsam { * ************************************************************************* */ template void ClusterTree::print(const std::string& str, - const boost::function& formatter) const { + const IndexFormatter& formatter) const { std::cout << str << std::endl; if (root_) root_->printTree("", formatter); } diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index f253fe4d6..276ef374d 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -78,10 +78,10 @@ namespace gtsam { Cluster(FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator); /// print - void print(const std::string& indent, const boost::function& formatter = &(boost::lexical_cast)) const; + void print(const std::string& indent, const IndexFormatter& formatter = DefaultIndexFormatter) const; /// print the enire tree - void printTree(const std::string& indent, const boost::function& formatter = &(boost::lexical_cast)) const; + void printTree(const std::string& indent, const IndexFormatter& formatter = DefaultIndexFormatter) const; /// check equality bool equals(const Cluster& other) const; @@ -127,7 +127,7 @@ namespace gtsam { /// @{ /// print the object - void print(const std::string& str="", const boost::function& formatter = &(boost::lexical_cast)) const; + void print(const std::string& str="", const IndexFormatter& formatter = DefaultIndexFormatter) const; /** check equality */ bool equals(const ClusterTree& other, double tol = 1e-9) const; diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 97f885596..8363c1800 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -131,8 +131,7 @@ public: /// @{ /** print with optional formatter */ - void print(const std::string& s = "Conditional", - const boost::function& formatter = &(boost::lexical_cast) ) const; + void print(const std::string& s = "Conditional", const IndexFormatter& formatter = DefaultIndexFormatter) const; /** check equality */ template @@ -201,7 +200,7 @@ private: /* ************************************************************************* */ template -void Conditional::print(const std::string& s, const boost::function& formatter) const { +void Conditional::print(const std::string& s, const IndexFormatter& formatter) const { std::cout << s << " P("; BOOST_FOREACH(KeyType key, frontals()) std::cout << " " << formatter(key); if (nrParents()>0) std::cout << " |"; diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index 81c98c926..2e977aa4b 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -167,7 +167,7 @@ EliminationTree::Create(const FactorGraph& factorGraph) { /* ************************************************************************* */ template void EliminationTree::print(const std::string& name, - const boost::function& formatter) const { + const IndexFormatter& formatter) const { std::cout << name << " (" << formatter(key_) << ")" << std::endl; BOOST_FOREACH(const sharedFactor& factor, factors_) { factor->print(name + " ", formatter); } diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index 8e57a7208..495fb40c8 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -144,7 +144,7 @@ public: /** Print the tree to cout */ void print(const std::string& name = "EliminationTree: ", - const boost::function& formatter = &(boost::lexical_cast)) const; + const IndexFormatter& formatter = DefaultIndexFormatter) const; /** Test whether the tree is equal to another */ bool equals(const EliminationTree& other, double tol = 1e-9) const; diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h index ad9f43701..6636b646c 100644 --- a/gtsam/inference/Factor-inl.h +++ b/gtsam/inference/Factor-inl.h @@ -57,7 +57,7 @@ namespace gtsam { /* ************************************************************************* */ template void Factor::print(const std::string& s, - const boost::function& formatter) const { + const IndexFormatter& formatter) const { std::cout << s << " "; BOOST_FOREACH(KEY key, keys_) std::cout << " " << formatter(key); std::cout << std::endl; diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 0e80fc8ea..d81c911a3 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -27,6 +27,7 @@ #include #include #include +#include #include namespace gtsam { @@ -193,7 +194,7 @@ public: /// print void print(const std::string& s = "Factor", - const boost::function& formatter = &(boost::lexical_cast)) const; + const IndexFormatter& formatter = DefaultIndexFormatter) const; /// check equality bool equals(const This& other, double tol = 1e-9) const; diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index c787655af..caf18d60e 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -49,7 +49,7 @@ namespace gtsam { /* ************************************************************************* */ template void FactorGraph::print(const std::string& s, - const boost::function& formatter) const { + const IndexFormatter& formatter) const { std::cout << s << std::endl; std::cout << "size: " << size() << std::endl; for (size_t i = 0; i < factors_.size(); i++) { diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 905afc55c..d0b9952d5 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -139,7 +139,7 @@ template class BayesTree; /** print out graph */ void print(const std::string& s = "FactorGraph", - const boost::function& formatter = &(boost::lexical_cast)) const; + const IndexFormatter& formatter = DefaultIndexFormatter) const; /** Check equality */ bool equals(const This& fg, double tol = 1e-9) const; diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 68b537c2d..5b7801156 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -139,7 +139,7 @@ public: /** print */ void print(const std::string& = "GaussianConditional", - const IndexFormatter& formatter = &(boost::lexical_cast)) const; + const IndexFormatter& formatter = DefaultIndexFormatter) const; /** equals function */ bool equals(const GaussianConditional &cg, double tol = 1e-9) const; diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 61db582f4..4d4f673f9 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -55,7 +55,7 @@ namespace gtsam { /// print void print(const std::string& = "GaussianDensity", - const IndexFormatter& formatter = &(boost::lexical_cast)) const; + const IndexFormatter& formatter =DefaultIndexFormatter) const; /// Mean \f$ \mu = R^{-1} d \f$ Vector mean() const; diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 64e4e970c..a20c09a43 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -84,7 +84,7 @@ namespace gtsam { // Implementing Testable interface virtual void print(const std::string& s = "", - const IndexFormatter& formatter = &(boost::lexical_cast)) const = 0; + const IndexFormatter& formatter = DefaultIndexFormatter) const = 0; virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0; diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 92f5c06d0..f90a33c5e 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -219,7 +219,7 @@ namespace gtsam { /** Print the factor for debugging and testing (implementing Testable) */ virtual void print(const std::string& s = "", - const IndexFormatter& formatter = &(boost::lexical_cast)) const; + const IndexFormatter& formatter = DefaultIndexFormatter) const; /** Compare to another factor for testing (implementing Testable) */ virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 14d073576..0ac085987 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -152,7 +152,7 @@ namespace gtsam { // Implementing Testable interface virtual void print(const std::string& s = "", - const IndexFormatter& formatter = &(boost::lexical_cast)) const; + const IndexFormatter& formatter = DefaultIndexFormatter) const; virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 89c59a6ad..04c280577 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -169,7 +169,7 @@ namespace gtsam { /** print required by Testable for unit testing */ void print(const std::string& str = "VectorValues: ", - const IndexFormatter& formatter = &(boost::lexical_cast)) const; + const IndexFormatter& formatter =DefaultIndexFormatter) const; /** equals required by Testable for unit testing */ bool equals(const VectorValues& x, double tol = 1e-9) const; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 00c8a026c..c118cb44e 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -287,7 +287,7 @@ public: /** print this node */ void print(const std::string& s = "", - const IndexFormatter& formatter = &(boost::lexical_cast)) const { + const IndexFormatter& formatter = DefaultIndexFormatter) const { Base::print(s,formatter); if(cachedFactor_) cachedFactor_->print(s + "Cached: ", formatter); From 3ab79d159762a21de05167691c870d7e52609105 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 28 Jun 2012 16:13:22 +0000 Subject: [PATCH 08/21] Fixed wrap test --- wrap/tests/expected/@Point2/Point2.m | 12 +- wrap/tests/expected/@Point2/argChar.cpp | 10 +- wrap/tests/expected/@Point2/argChar.m | 3 +- wrap/tests/expected/@Point2/argUChar.cpp | 10 +- wrap/tests/expected/@Point2/argUChar.m | 3 +- wrap/tests/expected/@Point2/dim.cpp | 8 +- wrap/tests/expected/@Point2/dim.m | 3 +- wrap/tests/expected/@Point2/returnChar.cpp | 8 +- wrap/tests/expected/@Point2/returnChar.m | 3 +- .../expected/@Point2/vectorConfusion.cpp | 12 +- wrap/tests/expected/@Point2/vectorConfusion.m | 3 +- wrap/tests/expected/@Point2/x.cpp | 8 +- wrap/tests/expected/@Point2/x.m | 3 +- wrap/tests/expected/@Point2/y.cpp | 8 +- wrap/tests/expected/@Point2/y.m | 3 +- wrap/tests/expected/@Point3/Point3.m | 10 +- wrap/tests/expected/@Point3/norm.cpp | 8 +- wrap/tests/expected/@Test/Test.m | 12 +- .../expected/@Test/arg_EigenConstRef.cpp | 10 +- wrap/tests/expected/@Test/arg_EigenConstRef.m | 3 +- .../tests/expected/@Test/create_MixedPtrs.cpp | 16 ++- wrap/tests/expected/@Test/create_MixedPtrs.m | 3 +- wrap/tests/expected/@Test/create_ptrs.cpp | 16 ++- wrap/tests/expected/@Test/create_ptrs.m | 3 +- wrap/tests/expected/@Test/print.cpp | 8 +- .../tests/expected/@Test/return_Point2Ptr.cpp | 14 +- wrap/tests/expected/@Test/return_Point2Ptr.m | 3 +- wrap/tests/expected/@Test/return_Test.cpp | 12 +- wrap/tests/expected/@Test/return_Test.m | 3 +- wrap/tests/expected/@Test/return_TestPtr.cpp | 12 +- wrap/tests/expected/@Test/return_TestPtr.m | 3 +- wrap/tests/expected/@Test/return_bool.cpp | 10 +- wrap/tests/expected/@Test/return_bool.m | 3 +- wrap/tests/expected/@Test/return_double.cpp | 10 +- wrap/tests/expected/@Test/return_double.m | 3 +- wrap/tests/expected/@Test/return_field.cpp | 10 +- wrap/tests/expected/@Test/return_field.m | 3 +- wrap/tests/expected/@Test/return_int.cpp | 10 +- wrap/tests/expected/@Test/return_int.m | 3 +- wrap/tests/expected/@Test/return_matrix1.cpp | 10 +- wrap/tests/expected/@Test/return_matrix1.m | 3 +- wrap/tests/expected/@Test/return_matrix2.cpp | 10 +- wrap/tests/expected/@Test/return_matrix2.m | 3 +- wrap/tests/expected/@Test/return_pair.cpp | 12 +- wrap/tests/expected/@Test/return_pair.m | 3 +- wrap/tests/expected/@Test/return_ptrs.cpp | 16 ++- wrap/tests/expected/@Test/return_ptrs.m | 3 +- wrap/tests/expected/@Test/return_size_t.cpp | 10 +- wrap/tests/expected/@Test/return_size_t.m | 3 +- wrap/tests/expected/@Test/return_string.cpp | 10 +- wrap/tests/expected/@Test/return_string.m | 3 +- wrap/tests/expected/@Test/return_vector1.cpp | 10 +- wrap/tests/expected/@Test/return_vector1.m | 3 +- wrap/tests/expected/@Test/return_vector2.cpp | 10 +- wrap/tests/expected/@Test/return_vector2.m | 3 +- wrap/tests/expected/Makefile | 20 +-- .../expected/Point3_StaticFunctionRet.cpp | 8 +- wrap/tests/expected/Point3_staticFunction.cpp | 3 + wrap/tests/expected/delete_Point2.cpp | 8 -- wrap/tests/expected/delete_Point2.m | 4 - wrap/tests/expected/delete_Point3.cpp | 9 -- wrap/tests/expected/delete_Point3.m | 4 - wrap/tests/expected/delete_Test.cpp | 9 -- wrap/tests/expected/delete_Test.m | 4 - wrap/tests/expected/make_geometry.m | 7 +- wrap/tests/expected/new_Point2_.cpp | 45 ++++++- wrap/tests/expected/new_Point2_.m | 2 +- wrap/tests/expected/new_Point2_dd.cpp | 11 -- wrap/tests/expected/new_Point2_dd.m | 4 - wrap/tests/expected/new_Point3_.cpp | 47 +++++++ wrap/tests/expected/new_Point3_.m | 4 + wrap/tests/expected/new_Point3_ddd.cpp | 13 -- wrap/tests/expected/new_Point3_ddd.m | 4 - wrap/tests/expected/new_Test_.cpp | 45 ++++++- wrap/tests/expected/new_Test_.m | 2 +- wrap/tests/expected/new_Test_dM.cpp | 12 -- wrap/tests/expected/new_Test_dM.m | 4 - .../expected_namespaces/@ClassD/ClassD.m | 10 +- .../@ns1ClassA/ns1ClassA.m | 10 +- .../@ns1ClassB/ns1ClassB.m | 10 +- .../@ns2ClassA/memberFunction.cpp | 8 +- .../@ns2ClassA/ns2ClassA.m | 10 +- .../expected_namespaces/@ns2ClassA/nsArg.cpp | 10 +- .../@ns2ClassA/nsReturn.cpp | 14 +- .../@ns2ClassC/ns2ClassC.m | 10 +- .../@ns2ns3ClassB/ns2ns3ClassB.m | 10 +- wrap/tests/expected_namespaces/Makefile | 24 +--- .../expected_namespaces/delete_ClassD.cpp | 8 -- .../tests/expected_namespaces/delete_ClassD.m | 4 - .../expected_namespaces/delete_ns1ClassA.cpp | 8 -- .../expected_namespaces/delete_ns1ClassA.m | 4 - .../expected_namespaces/delete_ns1ClassB.cpp | 9 -- .../expected_namespaces/delete_ns1ClassB.m | 4 - .../expected_namespaces/delete_ns2ClassA.cpp | 9 -- .../expected_namespaces/delete_ns2ClassA.m | 4 - .../expected_namespaces/delete_ns2ClassC.cpp | 8 -- .../expected_namespaces/delete_ns2ClassC.m | 4 - .../delete_ns2ns3ClassB.cpp | 9 -- .../expected_namespaces/delete_ns2ns3ClassB.m | 4 - .../expected_namespaces/make_testNamespaces.m | 6 - .../tests/expected_namespaces/new_ClassD_.cpp | 40 +++++- .../expected_namespaces/new_ns1ClassA_.cpp | 40 +++++- .../expected_namespaces/new_ns1ClassB_.cpp | 40 +++++- .../expected_namespaces/new_ns2ClassA_.cpp | 40 +++++- .../expected_namespaces/new_ns2ClassC_.cpp | 40 +++++- .../expected_namespaces/new_ns2ns3ClassB_.cpp | 40 +++++- .../ns2ClassA_afunction.cpp | 3 + wrap/tests/testWrap.cpp | 122 ++++++++++++------ 108 files changed, 783 insertions(+), 445 deletions(-) delete mode 100644 wrap/tests/expected/delete_Point2.cpp delete mode 100644 wrap/tests/expected/delete_Point2.m delete mode 100644 wrap/tests/expected/delete_Point3.cpp delete mode 100644 wrap/tests/expected/delete_Point3.m delete mode 100644 wrap/tests/expected/delete_Test.cpp delete mode 100644 wrap/tests/expected/delete_Test.m delete mode 100644 wrap/tests/expected/new_Point2_dd.cpp delete mode 100644 wrap/tests/expected/new_Point2_dd.m create mode 100644 wrap/tests/expected/new_Point3_.cpp create mode 100644 wrap/tests/expected/new_Point3_.m delete mode 100644 wrap/tests/expected/new_Point3_ddd.cpp delete mode 100644 wrap/tests/expected/new_Point3_ddd.m delete mode 100644 wrap/tests/expected/new_Test_dM.cpp delete mode 100644 wrap/tests/expected/new_Test_dM.m delete mode 100644 wrap/tests/expected_namespaces/delete_ClassD.cpp delete mode 100644 wrap/tests/expected_namespaces/delete_ClassD.m delete mode 100644 wrap/tests/expected_namespaces/delete_ns1ClassA.cpp delete mode 100644 wrap/tests/expected_namespaces/delete_ns1ClassA.m delete mode 100644 wrap/tests/expected_namespaces/delete_ns1ClassB.cpp delete mode 100644 wrap/tests/expected_namespaces/delete_ns1ClassB.m delete mode 100644 wrap/tests/expected_namespaces/delete_ns2ClassA.cpp delete mode 100644 wrap/tests/expected_namespaces/delete_ns2ClassA.m delete mode 100644 wrap/tests/expected_namespaces/delete_ns2ClassC.cpp delete mode 100644 wrap/tests/expected_namespaces/delete_ns2ClassC.m delete mode 100644 wrap/tests/expected_namespaces/delete_ns2ns3ClassB.cpp delete mode 100644 wrap/tests/expected_namespaces/delete_ns2ns3ClassB.m diff --git a/wrap/tests/expected/@Point2/Point2.m b/wrap/tests/expected/@Point2/Point2.m index 454d81399..68744ae00 100644 --- a/wrap/tests/expected/@Point2/Point2.m +++ b/wrap/tests/expected/@Point2/Point2.m @@ -5,12 +5,16 @@ classdef Point2 < handle end methods function obj = Point2(varargin) - if (nargin == 0), obj.self = new_Point2_(); end - if (nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')), obj.self = new_Point2_dd(varargin{1},varargin{2}); end - if nargin ~= 13 && obj.self == 0, error('Point2 constructor failed'); end + if (nargin == 0), obj.self = new_Point2_(0,0); end + if (nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')), obj.self = new_Point2_(0,1,varargin{1},varargin{2}); end + if nargin ==14, new_Point2_(varargin{1},0); end + if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('Point2 constructor failed'); end end function delete(obj) - delete_Point2(obj); + if obj.self ~= 0 + new_Point2_(obj.self); + obj.self = 0; + end end function display(obj), obj.print(''); end function disp(obj), obj.display; end diff --git a/wrap/tests/expected/@Point2/argChar.cpp b/wrap/tests/expected/@Point2/argChar.cpp index 18548ee8f..e5fac45fe 100644 --- a/wrap/tests/expected/@Point2/argChar.cpp +++ b/wrap/tests/expected/@Point2/argChar.cpp @@ -1,10 +1,14 @@ // automatically generated by wrap #include +#include #include +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("argChar",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); - char a = unwrap< char >(in[1]); - self->argChar(a); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + char a = unwrap< char >(in[1]); + obj->argChar(a); } diff --git a/wrap/tests/expected/@Point2/argChar.m b/wrap/tests/expected/@Point2/argChar.m index 93880c5b1..6c935a1d6 100644 --- a/wrap/tests/expected/@Point2/argChar.m +++ b/wrap/tests/expected/@Point2/argChar.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.argChar(a) function result = argChar(obj,a) -% usage: obj.argChar(a) error('need to compile argChar.cpp'); end diff --git a/wrap/tests/expected/@Point2/argUChar.cpp b/wrap/tests/expected/@Point2/argUChar.cpp index bbaa65a8f..54c592915 100644 --- a/wrap/tests/expected/@Point2/argUChar.cpp +++ b/wrap/tests/expected/@Point2/argUChar.cpp @@ -1,10 +1,14 @@ // automatically generated by wrap #include +#include #include +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("argUChar",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); - unsigned char a = unwrap< unsigned char >(in[1]); - self->argUChar(a); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + unsigned char a = unwrap< unsigned char >(in[1]); + obj->argUChar(a); } diff --git a/wrap/tests/expected/@Point2/argUChar.m b/wrap/tests/expected/@Point2/argUChar.m index bb524b3f0..ea42a2b4f 100644 --- a/wrap/tests/expected/@Point2/argUChar.m +++ b/wrap/tests/expected/@Point2/argUChar.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.argUChar(a) function result = argUChar(obj,a) -% usage: obj.argUChar(a) error('need to compile argUChar.cpp'); end diff --git a/wrap/tests/expected/@Point2/dim.cpp b/wrap/tests/expected/@Point2/dim.cpp index 1349dc267..7e44ae075 100644 --- a/wrap/tests/expected/@Point2/dim.cpp +++ b/wrap/tests/expected/@Point2/dim.cpp @@ -1,10 +1,14 @@ // automatically generated by wrap #include +#include #include +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("dim",nargout,nargin-1,0); - boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); - int result = self->dim(); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + int result = obj->dim(); out[0] = wrap< int >(result); } diff --git a/wrap/tests/expected/@Point2/dim.m b/wrap/tests/expected/@Point2/dim.m index 84c368193..934e0b895 100644 --- a/wrap/tests/expected/@Point2/dim.m +++ b/wrap/tests/expected/@Point2/dim.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.dim() function result = dim(obj) -% usage: obj.dim() error('need to compile dim.cpp'); end diff --git a/wrap/tests/expected/@Point2/returnChar.cpp b/wrap/tests/expected/@Point2/returnChar.cpp index c5b67a018..43a537786 100644 --- a/wrap/tests/expected/@Point2/returnChar.cpp +++ b/wrap/tests/expected/@Point2/returnChar.cpp @@ -1,10 +1,14 @@ // automatically generated by wrap #include +#include #include +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("returnChar",nargout,nargin-1,0); - boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); - char result = self->returnChar(); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + char result = obj->returnChar(); out[0] = wrap< char >(result); } diff --git a/wrap/tests/expected/@Point2/returnChar.m b/wrap/tests/expected/@Point2/returnChar.m index a33718047..8c3ceee35 100644 --- a/wrap/tests/expected/@Point2/returnChar.m +++ b/wrap/tests/expected/@Point2/returnChar.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.returnChar() function result = returnChar(obj) -% usage: obj.returnChar() error('need to compile returnChar.cpp'); end diff --git a/wrap/tests/expected/@Point2/vectorConfusion.cpp b/wrap/tests/expected/@Point2/vectorConfusion.cpp index d992d1d94..e3aa4f0d6 100644 --- a/wrap/tests/expected/@Point2/vectorConfusion.cpp +++ b/wrap/tests/expected/@Point2/vectorConfusion.cpp @@ -1,10 +1,16 @@ // automatically generated by wrap #include +#include #include +typedef boost::shared_ptr SharedVectorNotEigen; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("vectorConfusion",nargout,nargin-1,0); - boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); - VectorNotEigen result = self->vectorConfusion(); - out[0] = wrap_shared_ptr(boost::make_shared< VectorNotEigen >(result),"VectorNotEigen"); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + VectorNotEigen result = obj->vectorConfusion(); + SharedVectorNotEigen* ret = new SharedVectorNotEigen(new VectorNotEigen(result)); + out[0] = wrap_collect_shared_ptr(ret,"VectorNotEigen"); } diff --git a/wrap/tests/expected/@Point2/vectorConfusion.m b/wrap/tests/expected/@Point2/vectorConfusion.m index cc47b0dc7..9966c930d 100644 --- a/wrap/tests/expected/@Point2/vectorConfusion.m +++ b/wrap/tests/expected/@Point2/vectorConfusion.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.vectorConfusion() function result = vectorConfusion(obj) -% usage: obj.vectorConfusion() error('need to compile vectorConfusion.cpp'); end diff --git a/wrap/tests/expected/@Point2/x.cpp b/wrap/tests/expected/@Point2/x.cpp index 65e56cae5..8cebadb66 100644 --- a/wrap/tests/expected/@Point2/x.cpp +++ b/wrap/tests/expected/@Point2/x.cpp @@ -1,10 +1,14 @@ // automatically generated by wrap #include +#include #include +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("x",nargout,nargin-1,0); - boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); - double result = self->x(); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + double result = obj->x(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/@Point2/x.m b/wrap/tests/expected/@Point2/x.m index e1ebbd450..44f069872 100644 --- a/wrap/tests/expected/@Point2/x.m +++ b/wrap/tests/expected/@Point2/x.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.x() function result = x(obj) -% usage: obj.x() error('need to compile x.cpp'); end diff --git a/wrap/tests/expected/@Point2/y.cpp b/wrap/tests/expected/@Point2/y.cpp index f8e10dc5d..7e3650534 100644 --- a/wrap/tests/expected/@Point2/y.cpp +++ b/wrap/tests/expected/@Point2/y.cpp @@ -1,10 +1,14 @@ // automatically generated by wrap #include +#include #include +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("y",nargout,nargin-1,0); - boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); - double result = self->y(); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + double result = obj->y(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/@Point2/y.m b/wrap/tests/expected/@Point2/y.m index d27fc8bf2..7971c1e33 100644 --- a/wrap/tests/expected/@Point2/y.m +++ b/wrap/tests/expected/@Point2/y.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.y() function result = y(obj) -% usage: obj.y() error('need to compile y.cpp'); end diff --git a/wrap/tests/expected/@Point3/Point3.m b/wrap/tests/expected/@Point3/Point3.m index 3e7a6fd65..b916293b7 100644 --- a/wrap/tests/expected/@Point3/Point3.m +++ b/wrap/tests/expected/@Point3/Point3.m @@ -5,11 +5,15 @@ classdef Point3 < handle end methods function obj = Point3(varargin) - if (nargin == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double')), obj.self = new_Point3_ddd(varargin{1},varargin{2},varargin{3}); end - if nargin ~= 13 && obj.self == 0, error('Point3 constructor failed'); end + if (nargin == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double')), obj.self = new_Point3_(0,0,varargin{1},varargin{2},varargin{3}); end + if nargin ==14, new_Point3_(varargin{1},0); end + if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('Point3 constructor failed'); end end function delete(obj) - delete_Point3(obj); + if obj.self ~= 0 + new_Point3_(obj.self); + obj.self = 0; + end end function display(obj), obj.print(''); end function disp(obj), obj.display; end diff --git a/wrap/tests/expected/@Point3/norm.cpp b/wrap/tests/expected/@Point3/norm.cpp index 0c7ac2038..8f6a10b72 100644 --- a/wrap/tests/expected/@Point3/norm.cpp +++ b/wrap/tests/expected/@Point3/norm.cpp @@ -1,11 +1,15 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("norm",nargout,nargin-1,0); - boost::shared_ptr self = unwrap_shared_ptr< Point3 >(in[0],"Point3"); - double result = self->norm(); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + double result = obj->norm(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/@Test/Test.m b/wrap/tests/expected/@Test/Test.m index b8e55381c..e4bd30221 100644 --- a/wrap/tests/expected/@Test/Test.m +++ b/wrap/tests/expected/@Test/Test.m @@ -5,12 +5,16 @@ classdef Test < handle end methods function obj = Test(varargin) - if (nargin == 0), obj.self = new_Test_(); end - if (nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')), obj.self = new_Test_dM(varargin{1},varargin{2}); end - if nargin ~= 13 && obj.self == 0, error('Test constructor failed'); end + if (nargin == 0), obj.self = new_Test_(0,0); end + if (nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')), obj.self = new_Test_(0,1,varargin{1},varargin{2}); end + if nargin ==14, new_Test_(varargin{1},0); end + if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('Test constructor failed'); end end function delete(obj) - delete_Test(obj); + if obj.self ~= 0 + new_Test_(obj.self); + obj.self = 0; + end end function display(obj), obj.print(''); end function disp(obj), obj.display; end diff --git a/wrap/tests/expected/@Test/arg_EigenConstRef.cpp b/wrap/tests/expected/@Test/arg_EigenConstRef.cpp index 09a5c6f62..848870d11 100644 --- a/wrap/tests/expected/@Test/arg_EigenConstRef.cpp +++ b/wrap/tests/expected/@Test/arg_EigenConstRef.cpp @@ -1,11 +1,15 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("arg_EigenConstRef",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "Matrix"); - self->arg_EigenConstRef(value); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "Matrix"); + obj->arg_EigenConstRef(value); } diff --git a/wrap/tests/expected/@Test/arg_EigenConstRef.m b/wrap/tests/expected/@Test/arg_EigenConstRef.m index 9e6c04c5a..c348014c1 100644 --- a/wrap/tests/expected/@Test/arg_EigenConstRef.m +++ b/wrap/tests/expected/@Test/arg_EigenConstRef.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.arg_EigenConstRef(value) function result = arg_EigenConstRef(obj,value) -% usage: obj.arg_EigenConstRef(value) error('need to compile arg_EigenConstRef.cpp'); end diff --git a/wrap/tests/expected/@Test/create_MixedPtrs.cpp b/wrap/tests/expected/@Test/create_MixedPtrs.cpp index 81bcdc5d8..1776c855b 100644 --- a/wrap/tests/expected/@Test/create_MixedPtrs.cpp +++ b/wrap/tests/expected/@Test/create_MixedPtrs.cpp @@ -1,12 +1,20 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr SharedTest; +typedef boost::shared_ptr SharedTest; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_MixedPtrs",nargout,nargin-1,0); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - pair< Test, boost::shared_ptr > result = self->create_MixedPtrs(); - out[0] = wrap_shared_ptr(boost::make_shared< Test >(result.first),"Test"); - out[1] = wrap_shared_ptr(result.second,"Test"); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + pair< Test, SharedTest > result = obj->create_MixedPtrs(); + SharedTest* ret = new SharedTest(new Test(result.first)); + out[0] = wrap_collect_shared_ptr(ret,"Test"); + SharedTest* ret = new SharedTest(result.second); + out[1] = wrap_collect_shared_ptr(ret,"Test"); } diff --git a/wrap/tests/expected/@Test/create_MixedPtrs.m b/wrap/tests/expected/@Test/create_MixedPtrs.m index bd1927fba..38a9f1d7e 100644 --- a/wrap/tests/expected/@Test/create_MixedPtrs.m +++ b/wrap/tests/expected/@Test/create_MixedPtrs.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% [first,second] = obj.create_MixedPtrs() function [first,second] = create_MixedPtrs(obj) -% usage: obj.create_MixedPtrs() error('need to compile create_MixedPtrs.cpp'); end diff --git a/wrap/tests/expected/@Test/create_ptrs.cpp b/wrap/tests/expected/@Test/create_ptrs.cpp index 830d62a12..ab4261d5c 100644 --- a/wrap/tests/expected/@Test/create_ptrs.cpp +++ b/wrap/tests/expected/@Test/create_ptrs.cpp @@ -1,12 +1,20 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr SharedTest; +typedef boost::shared_ptr SharedTest; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_ptrs",nargout,nargin-1,0); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - pair< boost::shared_ptr, boost::shared_ptr > result = self->create_ptrs(); - out[0] = wrap_shared_ptr(result.first,"Test"); - out[1] = wrap_shared_ptr(result.second,"Test"); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + pair< SharedTest, SharedTest > result = obj->create_ptrs(); + SharedTest* ret = new SharedTest(result.first); + out[0] = wrap_collect_shared_ptr(ret,"Test"); + SharedTest* ret = new SharedTest(result.second); + out[1] = wrap_collect_shared_ptr(ret,"Test"); } diff --git a/wrap/tests/expected/@Test/create_ptrs.m b/wrap/tests/expected/@Test/create_ptrs.m index e380f1829..80c6781dc 100644 --- a/wrap/tests/expected/@Test/create_ptrs.m +++ b/wrap/tests/expected/@Test/create_ptrs.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% [first,second] = obj.create_ptrs() function [first,second] = create_ptrs(obj) -% usage: obj.create_ptrs() error('need to compile create_ptrs.cpp'); end diff --git a/wrap/tests/expected/@Test/print.cpp b/wrap/tests/expected/@Test/print.cpp index 1d259f2e8..e3a758182 100644 --- a/wrap/tests/expected/@Test/print.cpp +++ b/wrap/tests/expected/@Test/print.cpp @@ -1,10 +1,14 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - self->print(); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + obj->print(); } diff --git a/wrap/tests/expected/@Test/return_Point2Ptr.cpp b/wrap/tests/expected/@Test/return_Point2Ptr.cpp index e6990198e..b3e61d37a 100644 --- a/wrap/tests/expected/@Test/return_Point2Ptr.cpp +++ b/wrap/tests/expected/@Test/return_Point2Ptr.cpp @@ -1,12 +1,18 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr SharedPoint2; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Point2Ptr",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - bool value = unwrap< bool >(in[1]); - boost::shared_ptr result = self->return_Point2Ptr(value); - out[0] = wrap_shared_ptr(result,"Point2"); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + bool value = unwrap< bool >(in[1]); + SharedPoint2 result = obj->return_Point2Ptr(value); + SharedPoint2* ret = new SharedPoint2(result); + out[0] = wrap_collect_shared_ptr(ret,"Point2"); } diff --git a/wrap/tests/expected/@Test/return_Point2Ptr.m b/wrap/tests/expected/@Test/return_Point2Ptr.m index 26fd146a2..84e586bc7 100644 --- a/wrap/tests/expected/@Test/return_Point2Ptr.m +++ b/wrap/tests/expected/@Test/return_Point2Ptr.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.return_Point2Ptr(value) function result = return_Point2Ptr(obj,value) -% usage: obj.return_Point2Ptr(value) error('need to compile return_Point2Ptr.cpp'); end diff --git a/wrap/tests/expected/@Test/return_Test.cpp b/wrap/tests/expected/@Test/return_Test.cpp index 63e9f5a3b..cd0f1ef10 100644 --- a/wrap/tests/expected/@Test/return_Test.cpp +++ b/wrap/tests/expected/@Test/return_Test.cpp @@ -1,12 +1,18 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr SharedTest; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Test",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "Test"); - Test result = self->return_Test(value); - out[0] = wrap_shared_ptr(boost::make_shared< Test >(result),"Test"); + Test result = obj->return_Test(value); + SharedTest* ret = new SharedTest(new Test(result)); + out[0] = wrap_collect_shared_ptr(ret,"Test"); } diff --git a/wrap/tests/expected/@Test/return_Test.m b/wrap/tests/expected/@Test/return_Test.m index f31dc3192..d1a2e440c 100644 --- a/wrap/tests/expected/@Test/return_Test.m +++ b/wrap/tests/expected/@Test/return_Test.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.return_Test(value) function result = return_Test(obj,value) -% usage: obj.return_Test(value) error('need to compile return_Test.cpp'); end diff --git a/wrap/tests/expected/@Test/return_TestPtr.cpp b/wrap/tests/expected/@Test/return_TestPtr.cpp index 3c053791d..2957de8f3 100644 --- a/wrap/tests/expected/@Test/return_TestPtr.cpp +++ b/wrap/tests/expected/@Test/return_TestPtr.cpp @@ -1,12 +1,18 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr SharedTest; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_TestPtr",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "Test"); - boost::shared_ptr result = self->return_TestPtr(value); - out[0] = wrap_shared_ptr(result,"Test"); + SharedTest result = obj->return_TestPtr(value); + SharedTest* ret = new SharedTest(result); + out[0] = wrap_collect_shared_ptr(ret,"Test"); } diff --git a/wrap/tests/expected/@Test/return_TestPtr.m b/wrap/tests/expected/@Test/return_TestPtr.m index e69149551..937c85fcc 100644 --- a/wrap/tests/expected/@Test/return_TestPtr.m +++ b/wrap/tests/expected/@Test/return_TestPtr.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.return_TestPtr(value) function result = return_TestPtr(obj,value) -% usage: obj.return_TestPtr(value) error('need to compile return_TestPtr.cpp'); end diff --git a/wrap/tests/expected/@Test/return_bool.cpp b/wrap/tests/expected/@Test/return_bool.cpp index 92612a279..c9c792934 100644 --- a/wrap/tests/expected/@Test/return_bool.cpp +++ b/wrap/tests/expected/@Test/return_bool.cpp @@ -1,12 +1,16 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_bool",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - bool value = unwrap< bool >(in[1]); - bool result = self->return_bool(value); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + bool value = unwrap< bool >(in[1]); + bool result = obj->return_bool(value); out[0] = wrap< bool >(result); } diff --git a/wrap/tests/expected/@Test/return_bool.m b/wrap/tests/expected/@Test/return_bool.m index a0c5a0b17..358cb9750 100644 --- a/wrap/tests/expected/@Test/return_bool.m +++ b/wrap/tests/expected/@Test/return_bool.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.return_bool(value) function result = return_bool(obj,value) -% usage: obj.return_bool(value) error('need to compile return_bool.cpp'); end diff --git a/wrap/tests/expected/@Test/return_double.cpp b/wrap/tests/expected/@Test/return_double.cpp index e167a16c0..4e6612278 100644 --- a/wrap/tests/expected/@Test/return_double.cpp +++ b/wrap/tests/expected/@Test/return_double.cpp @@ -1,12 +1,16 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_double",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - double value = unwrap< double >(in[1]); - double result = self->return_double(value); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + double value = unwrap< double >(in[1]); + double result = obj->return_double(value); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/@Test/return_double.m b/wrap/tests/expected/@Test/return_double.m index dd181ff0b..681371f39 100644 --- a/wrap/tests/expected/@Test/return_double.m +++ b/wrap/tests/expected/@Test/return_double.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.return_double(value) function result = return_double(obj,value) -% usage: obj.return_double(value) error('need to compile return_double.cpp'); end diff --git a/wrap/tests/expected/@Test/return_field.cpp b/wrap/tests/expected/@Test/return_field.cpp index 838bab0a4..43507f4d7 100644 --- a/wrap/tests/expected/@Test/return_field.cpp +++ b/wrap/tests/expected/@Test/return_field.cpp @@ -1,12 +1,16 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_field",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - Test& t = *unwrap_shared_ptr< Test >(in[1], "Test"); - bool result = self->return_field(t); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + Test& t = *unwrap_shared_ptr< Test >(in[1], "Test"); + bool result = obj->return_field(t); out[0] = wrap< bool >(result); } diff --git a/wrap/tests/expected/@Test/return_field.m b/wrap/tests/expected/@Test/return_field.m index bc4223671..e2894c381 100644 --- a/wrap/tests/expected/@Test/return_field.m +++ b/wrap/tests/expected/@Test/return_field.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.return_field(t) function result = return_field(obj,t) -% usage: obj.return_field(t) error('need to compile return_field.cpp'); end diff --git a/wrap/tests/expected/@Test/return_int.cpp b/wrap/tests/expected/@Test/return_int.cpp index 4cdaf5abc..2a27ac73a 100644 --- a/wrap/tests/expected/@Test/return_int.cpp +++ b/wrap/tests/expected/@Test/return_int.cpp @@ -1,12 +1,16 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_int",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - int value = unwrap< int >(in[1]); - int result = self->return_int(value); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + int value = unwrap< int >(in[1]); + int result = obj->return_int(value); out[0] = wrap< int >(result); } diff --git a/wrap/tests/expected/@Test/return_int.m b/wrap/tests/expected/@Test/return_int.m index 4984557e6..779e9feb2 100644 --- a/wrap/tests/expected/@Test/return_int.m +++ b/wrap/tests/expected/@Test/return_int.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.return_int(value) function result = return_int(obj,value) -% usage: obj.return_int(value) error('need to compile return_int.cpp'); end diff --git a/wrap/tests/expected/@Test/return_matrix1.cpp b/wrap/tests/expected/@Test/return_matrix1.cpp index f7fb72040..d4c66622b 100644 --- a/wrap/tests/expected/@Test/return_matrix1.cpp +++ b/wrap/tests/expected/@Test/return_matrix1.cpp @@ -1,12 +1,16 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix1",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - Matrix value = unwrap< Matrix >(in[1]); - Matrix result = self->return_matrix1(value); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + Matrix value = unwrap< Matrix >(in[1]); + Matrix result = obj->return_matrix1(value); out[0] = wrap< Matrix >(result); } diff --git a/wrap/tests/expected/@Test/return_matrix1.m b/wrap/tests/expected/@Test/return_matrix1.m index 66dd1886f..d6d9791f9 100644 --- a/wrap/tests/expected/@Test/return_matrix1.m +++ b/wrap/tests/expected/@Test/return_matrix1.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.return_matrix1(value) function result = return_matrix1(obj,value) -% usage: obj.return_matrix1(value) error('need to compile return_matrix1.cpp'); end diff --git a/wrap/tests/expected/@Test/return_matrix2.cpp b/wrap/tests/expected/@Test/return_matrix2.cpp index f8b6823fa..a40fa79cf 100644 --- a/wrap/tests/expected/@Test/return_matrix2.cpp +++ b/wrap/tests/expected/@Test/return_matrix2.cpp @@ -1,12 +1,16 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix2",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - Matrix value = unwrap< Matrix >(in[1]); - Matrix result = self->return_matrix2(value); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + Matrix value = unwrap< Matrix >(in[1]); + Matrix result = obj->return_matrix2(value); out[0] = wrap< Matrix >(result); } diff --git a/wrap/tests/expected/@Test/return_matrix2.m b/wrap/tests/expected/@Test/return_matrix2.m index 5a0359862..584b365b8 100644 --- a/wrap/tests/expected/@Test/return_matrix2.m +++ b/wrap/tests/expected/@Test/return_matrix2.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.return_matrix2(value) function result = return_matrix2(obj,value) -% usage: obj.return_matrix2(value) error('need to compile return_matrix2.cpp'); end diff --git a/wrap/tests/expected/@Test/return_pair.cpp b/wrap/tests/expected/@Test/return_pair.cpp index 54b3f6522..7d715644c 100644 --- a/wrap/tests/expected/@Test/return_pair.cpp +++ b/wrap/tests/expected/@Test/return_pair.cpp @@ -1,14 +1,18 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,2); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - Vector v = unwrap< Vector >(in[1]); - Matrix A = unwrap< Matrix >(in[2]); - pair< Vector, Matrix > result = self->return_pair(v,A); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + Vector v = unwrap< Vector >(in[1]); + Matrix A = unwrap< Matrix >(in[2]); + pair< Vector, Matrix > result = obj->return_pair(v,A); out[0] = wrap< Vector >(result.first); out[1] = wrap< Matrix >(result.second); } diff --git a/wrap/tests/expected/@Test/return_pair.m b/wrap/tests/expected/@Test/return_pair.m index 611dd3434..2e892210c 100644 --- a/wrap/tests/expected/@Test/return_pair.m +++ b/wrap/tests/expected/@Test/return_pair.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% [first,second] = obj.return_pair(v,A) function [first,second] = return_pair(obj,v,A) -% usage: obj.return_pair(v,A) error('need to compile return_pair.cpp'); end diff --git a/wrap/tests/expected/@Test/return_ptrs.cpp b/wrap/tests/expected/@Test/return_ptrs.cpp index eea94ca3c..8a5f9a0ce 100644 --- a/wrap/tests/expected/@Test/return_ptrs.cpp +++ b/wrap/tests/expected/@Test/return_ptrs.cpp @@ -1,14 +1,22 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr SharedTest; +typedef boost::shared_ptr SharedTest; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_ptrs",nargout,nargin-1,2); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; boost::shared_ptr p1 = unwrap_shared_ptr< Test >(in[1], "Test"); boost::shared_ptr p2 = unwrap_shared_ptr< Test >(in[2], "Test"); - pair< boost::shared_ptr, boost::shared_ptr > result = self->return_ptrs(p1,p2); - out[0] = wrap_shared_ptr(result.first,"Test"); - out[1] = wrap_shared_ptr(result.second,"Test"); + pair< SharedTest, SharedTest > result = obj->return_ptrs(p1,p2); + SharedTest* ret = new SharedTest(result.first); + out[0] = wrap_collect_shared_ptr(ret,"Test"); + SharedTest* ret = new SharedTest(result.second); + out[1] = wrap_collect_shared_ptr(ret,"Test"); } diff --git a/wrap/tests/expected/@Test/return_ptrs.m b/wrap/tests/expected/@Test/return_ptrs.m index 18d69ac92..a7af4b73c 100644 --- a/wrap/tests/expected/@Test/return_ptrs.m +++ b/wrap/tests/expected/@Test/return_ptrs.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% [first,second] = obj.return_ptrs(p1,p2) function [first,second] = return_ptrs(obj,p1,p2) -% usage: obj.return_ptrs(p1,p2) error('need to compile return_ptrs.cpp'); end diff --git a/wrap/tests/expected/@Test/return_size_t.cpp b/wrap/tests/expected/@Test/return_size_t.cpp index 901c5c9bd..f18680d0d 100644 --- a/wrap/tests/expected/@Test/return_size_t.cpp +++ b/wrap/tests/expected/@Test/return_size_t.cpp @@ -1,12 +1,16 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_size_t",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - size_t value = unwrap< size_t >(in[1]); - size_t result = self->return_size_t(value); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + size_t value = unwrap< size_t >(in[1]); + size_t result = obj->return_size_t(value); out[0] = wrap< size_t >(result); } diff --git a/wrap/tests/expected/@Test/return_size_t.m b/wrap/tests/expected/@Test/return_size_t.m index 143f04d24..2fae64028 100644 --- a/wrap/tests/expected/@Test/return_size_t.m +++ b/wrap/tests/expected/@Test/return_size_t.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.return_size_t(value) function result = return_size_t(obj,value) -% usage: obj.return_size_t(value) error('need to compile return_size_t.cpp'); end diff --git a/wrap/tests/expected/@Test/return_string.cpp b/wrap/tests/expected/@Test/return_string.cpp index 778e07522..013814241 100644 --- a/wrap/tests/expected/@Test/return_string.cpp +++ b/wrap/tests/expected/@Test/return_string.cpp @@ -1,12 +1,16 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_string",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - string value = unwrap< string >(in[1]); - string result = self->return_string(value); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + string value = unwrap< string >(in[1]); + string result = obj->return_string(value); out[0] = wrap< string >(result); } diff --git a/wrap/tests/expected/@Test/return_string.m b/wrap/tests/expected/@Test/return_string.m index f1eab8661..67fb5f10d 100644 --- a/wrap/tests/expected/@Test/return_string.m +++ b/wrap/tests/expected/@Test/return_string.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.return_string(value) function result = return_string(obj,value) -% usage: obj.return_string(value) error('need to compile return_string.cpp'); end diff --git a/wrap/tests/expected/@Test/return_vector1.cpp b/wrap/tests/expected/@Test/return_vector1.cpp index 5e8aed397..593cd20bd 100644 --- a/wrap/tests/expected/@Test/return_vector1.cpp +++ b/wrap/tests/expected/@Test/return_vector1.cpp @@ -1,12 +1,16 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector1",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - Vector value = unwrap< Vector >(in[1]); - Vector result = self->return_vector1(value); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + Vector value = unwrap< Vector >(in[1]); + Vector result = obj->return_vector1(value); out[0] = wrap< Vector >(result); } diff --git a/wrap/tests/expected/@Test/return_vector1.m b/wrap/tests/expected/@Test/return_vector1.m index 316ca4cf2..461c51618 100644 --- a/wrap/tests/expected/@Test/return_vector1.m +++ b/wrap/tests/expected/@Test/return_vector1.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.return_vector1(value) function result = return_vector1(obj,value) -% usage: obj.return_vector1(value) error('need to compile return_vector1.cpp'); end diff --git a/wrap/tests/expected/@Test/return_vector2.cpp b/wrap/tests/expected/@Test/return_vector2.cpp index 4c3242f2e..1be4a614a 100644 --- a/wrap/tests/expected/@Test/return_vector2.cpp +++ b/wrap/tests/expected/@Test/return_vector2.cpp @@ -1,12 +1,16 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector2",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); - Vector value = unwrap< Vector >(in[1]); - Vector result = self->return_vector2(value); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + Vector value = unwrap< Vector >(in[1]); + Vector result = obj->return_vector2(value); out[0] = wrap< Vector >(result); } diff --git a/wrap/tests/expected/@Test/return_vector2.m b/wrap/tests/expected/@Test/return_vector2.m index 6426e0ce9..f3c77dd85 100644 --- a/wrap/tests/expected/@Test/return_vector2.m +++ b/wrap/tests/expected/@Test/return_vector2.m @@ -1,5 +1,4 @@ -% automatically generated by wrap +% result = obj.return_vector2(value) function result = return_vector2(obj,value) -% usage: obj.return_vector2(value) error('need to compile return_vector2.cpp'); end diff --git a/wrap/tests/expected/Makefile b/wrap/tests/expected/Makefile index a8959757b..0797d0dd3 100644 --- a/wrap/tests/expected/Makefile +++ b/wrap/tests/expected/Makefile @@ -9,10 +9,6 @@ all: Point2 Point3 Test # Point2 new_Point2_.$(MEXENDING): new_Point2_.cpp $(MEX) $(mex_flags) new_Point2_.cpp -output new_Point2_ -new_Point2_dd.$(MEXENDING): new_Point2_dd.cpp - $(MEX) $(mex_flags) new_Point2_dd.cpp -output new_Point2_dd -delete_Point2.$(MEXENDING): delete_Point2.cpp - $(MEX) $(mex_flags) delete_Point2.cpp -output delete_Point2 @Point2/x.$(MEXENDING): @Point2/x.cpp $(MEX) $(mex_flags) @Point2/x.cpp -output @Point2/x @Point2/y.$(MEXENDING): @Point2/y.cpp @@ -28,13 +24,11 @@ delete_Point2.$(MEXENDING): delete_Point2.cpp @Point2/vectorConfusion.$(MEXENDING): @Point2/vectorConfusion.cpp $(MEX) $(mex_flags) @Point2/vectorConfusion.cpp -output @Point2/vectorConfusion -Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) delete_Point2.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING) @Point2/returnChar.$(MEXENDING) @Point2/argChar.$(MEXENDING) @Point2/argUChar.$(MEXENDING) @Point2/vectorConfusion.$(MEXENDING) +Point2: new_Point2_.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING) @Point2/returnChar.$(MEXENDING) @Point2/argChar.$(MEXENDING) @Point2/argUChar.$(MEXENDING) @Point2/vectorConfusion.$(MEXENDING) # Point3 -new_Point3_ddd.$(MEXENDING): new_Point3_ddd.cpp - $(MEX) $(mex_flags) new_Point3_ddd.cpp -output new_Point3_ddd -delete_Point3.$(MEXENDING): delete_Point3.cpp - $(MEX) $(mex_flags) delete_Point3.cpp -output delete_Point3 +new_Point3_.$(MEXENDING): new_Point3_.cpp + $(MEX) $(mex_flags) new_Point3_.cpp -output new_Point3_ Point3_staticFunction.$(MEXENDING): Point3_staticFunction.cpp $(MEX) $(mex_flags) Point3_staticFunction.cpp -output Point3_staticFunction Point3_StaticFunctionRet.$(MEXENDING): Point3_StaticFunctionRet.cpp @@ -42,15 +36,11 @@ Point3_StaticFunctionRet.$(MEXENDING): Point3_StaticFunctionRet.cpp @Point3/norm.$(MEXENDING): @Point3/norm.cpp $(MEX) $(mex_flags) @Point3/norm.cpp -output @Point3/norm -Point3: new_Point3_ddd.$(MEXENDING) delete_Point3.$(MEXENDING) Point3_staticFunction.$(MEXENDING) Point3_StaticFunctionRet.$(MEXENDING) @Point3/norm.$(MEXENDING) +Point3: new_Point3_.$(MEXENDING) Point3_staticFunction.$(MEXENDING) Point3_StaticFunctionRet.$(MEXENDING) @Point3/norm.$(MEXENDING) # Test new_Test_.$(MEXENDING): new_Test_.cpp $(MEX) $(mex_flags) new_Test_.cpp -output new_Test_ -new_Test_dM.$(MEXENDING): new_Test_dM.cpp - $(MEX) $(mex_flags) new_Test_dM.cpp -output new_Test_dM -delete_Test.$(MEXENDING): delete_Test.cpp - $(MEX) $(mex_flags) delete_Test.cpp -output delete_Test @Test/return_pair.$(MEXENDING): @Test/return_pair.cpp $(MEX) $(mex_flags) @Test/return_pair.cpp -output @Test/return_pair @Test/return_bool.$(MEXENDING): @Test/return_bool.cpp @@ -90,7 +80,7 @@ delete_Test.$(MEXENDING): delete_Test.cpp @Test/print.$(MEXENDING): @Test/print.cpp $(MEX) $(mex_flags) @Test/print.cpp -output @Test/print -Test: new_Test_.$(MEXENDING) new_Test_dM.$(MEXENDING) delete_Test.$(MEXENDING) @Test/return_pair.$(MEXENDING) @Test/return_bool.$(MEXENDING) @Test/return_size_t.$(MEXENDING) @Test/return_int.$(MEXENDING) @Test/return_double.$(MEXENDING) @Test/return_string.$(MEXENDING) @Test/return_vector1.$(MEXENDING) @Test/return_matrix1.$(MEXENDING) @Test/return_vector2.$(MEXENDING) @Test/return_matrix2.$(MEXENDING) @Test/arg_EigenConstRef.$(MEXENDING) @Test/return_field.$(MEXENDING) @Test/return_TestPtr.$(MEXENDING) @Test/return_Test.$(MEXENDING) @Test/return_Point2Ptr.$(MEXENDING) @Test/create_ptrs.$(MEXENDING) @Test/create_MixedPtrs.$(MEXENDING) @Test/return_ptrs.$(MEXENDING) @Test/print.$(MEXENDING) +Test: new_Test_.$(MEXENDING) @Test/return_pair.$(MEXENDING) @Test/return_bool.$(MEXENDING) @Test/return_size_t.$(MEXENDING) @Test/return_int.$(MEXENDING) @Test/return_double.$(MEXENDING) @Test/return_string.$(MEXENDING) @Test/return_vector1.$(MEXENDING) @Test/return_matrix1.$(MEXENDING) @Test/return_vector2.$(MEXENDING) @Test/return_matrix2.$(MEXENDING) @Test/arg_EigenConstRef.$(MEXENDING) @Test/return_field.$(MEXENDING) @Test/return_TestPtr.$(MEXENDING) @Test/return_Test.$(MEXENDING) @Test/return_Point2Ptr.$(MEXENDING) @Test/create_ptrs.$(MEXENDING) @Test/create_MixedPtrs.$(MEXENDING) @Test/return_ptrs.$(MEXENDING) @Test/print.$(MEXENDING) diff --git a/wrap/tests/expected/Point3_StaticFunctionRet.cpp b/wrap/tests/expected/Point3_StaticFunctionRet.cpp index 652d8713e..49d15f4fa 100644 --- a/wrap/tests/expected/Point3_StaticFunctionRet.cpp +++ b/wrap/tests/expected/Point3_StaticFunctionRet.cpp @@ -1,11 +1,15 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr SharedPoint3; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("Point3_StaticFunctionRet",nargout,nargin,1); - double z = unwrap< double >(in[0]); + double z = unwrap< double >(in[0]); Point3 result = Point3::StaticFunctionRet(z); - out[0] = wrap_shared_ptr(boost::make_shared< Point3 >(result),"Point3"); + SharedPoint3* ret = new SharedPoint3(new Point3(result)); + out[0] = wrap_collect_shared_ptr(ret,"Point3"); } diff --git a/wrap/tests/expected/Point3_staticFunction.cpp b/wrap/tests/expected/Point3_staticFunction.cpp index 6adfdbe5a..e29670b9a 100644 --- a/wrap/tests/expected/Point3_staticFunction.cpp +++ b/wrap/tests/expected/Point3_staticFunction.cpp @@ -1,7 +1,10 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shareddouble; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("Point3_staticFunction",nargout,nargin,0); diff --git a/wrap/tests/expected/delete_Point2.cpp b/wrap/tests/expected/delete_Point2.cpp deleted file mode 100644 index bd8bd12eb..000000000 --- a/wrap/tests/expected/delete_Point2.cpp +++ /dev/null @@ -1,8 +0,0 @@ -// automatically generated by wrap -#include -#include -void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("delete_Point2",nargout,nargin,1); - delete_shared_ptr< Point2 >(in[0],"Point2"); -} diff --git a/wrap/tests/expected/delete_Point2.m b/wrap/tests/expected/delete_Point2.m deleted file mode 100644 index c6c623bec..000000000 --- a/wrap/tests/expected/delete_Point2.m +++ /dev/null @@ -1,4 +0,0 @@ -% automatically generated by wrap -function result = delete_Point2(obj) - error('need to compile delete_Point2.cpp'); -end diff --git a/wrap/tests/expected/delete_Point3.cpp b/wrap/tests/expected/delete_Point3.cpp deleted file mode 100644 index 9836bb25e..000000000 --- a/wrap/tests/expected/delete_Point3.cpp +++ /dev/null @@ -1,9 +0,0 @@ -// automatically generated by wrap -#include -#include -using namespace geometry; -void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("delete_Point3",nargout,nargin,1); - delete_shared_ptr< Point3 >(in[0],"Point3"); -} diff --git a/wrap/tests/expected/delete_Point3.m b/wrap/tests/expected/delete_Point3.m deleted file mode 100644 index b52b898cf..000000000 --- a/wrap/tests/expected/delete_Point3.m +++ /dev/null @@ -1,4 +0,0 @@ -% automatically generated by wrap -function result = delete_Point3(obj) - error('need to compile delete_Point3.cpp'); -end diff --git a/wrap/tests/expected/delete_Test.cpp b/wrap/tests/expected/delete_Test.cpp deleted file mode 100644 index 6a22cc327..000000000 --- a/wrap/tests/expected/delete_Test.cpp +++ /dev/null @@ -1,9 +0,0 @@ -// automatically generated by wrap -#include -#include -using namespace geometry; -void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("delete_Test",nargout,nargin,1); - delete_shared_ptr< Test >(in[0],"Test"); -} diff --git a/wrap/tests/expected/delete_Test.m b/wrap/tests/expected/delete_Test.m deleted file mode 100644 index 21ec790a3..000000000 --- a/wrap/tests/expected/delete_Test.m +++ /dev/null @@ -1,4 +0,0 @@ -% automatically generated by wrap -function result = delete_Test(obj) - error('need to compile delete_Test.cpp'); -end diff --git a/wrap/tests/expected/make_geometry.m b/wrap/tests/expected/make_geometry.m index 2858f2994..32f81871d 100644 --- a/wrap/tests/expected/make_geometry.m +++ b/wrap/tests/expected/make_geometry.m @@ -10,8 +10,6 @@ addpath(toolboxpath); %% Point2 cd(toolboxpath) mex -O5 new_Point2_.cpp -mex -O5 new_Point2_dd.cpp -mex -O5 delete_Point2.cpp cd @Point2 mex -O5 x.cpp @@ -24,8 +22,7 @@ mex -O5 vectorConfusion.cpp %% Point3 cd(toolboxpath) -mex -O5 new_Point3_ddd.cpp -mex -O5 delete_Point3.cpp +mex -O5 new_Point3_.cpp mex -O5 Point3_staticFunction.cpp mex -O5 Point3_StaticFunctionRet.cpp @@ -35,8 +32,6 @@ mex -O5 norm.cpp %% Test cd(toolboxpath) mex -O5 new_Test_.cpp -mex -O5 new_Test_dM.cpp -mex -O5 delete_Test.cpp cd @Test mex -O5 return_pair.cpp diff --git a/wrap/tests/expected/new_Point2_.cpp b/wrap/tests/expected/new_Point2_.cpp index a88076efb..7d3af0038 100644 --- a/wrap/tests/expected/new_Point2_.cpp +++ b/wrap/tests/expected/new_Point2_.cpp @@ -1,9 +1,48 @@ // automatically generated by wrap #include +#include #include +typedef boost::shared_ptr Shared; + +static std::set collector; + +struct Destruct +{ + void operator() (Shared* p) + { + collector.erase(p); + } +}; + +void cleanup(void) { + std::for_each( collector.begin(), collector.end(), Destruct() ); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("new_Point2_",nargout,nargin,0); - Point2* self = new Point2(); - out[0] = wrap_constructed(self,"Point2"); + mexAtExit(cleanup); + + const mxArray* input = in[0]; + Shared* self = *(Shared**) mxGetData(input); + + if(self) { + if(nargin > 1) { + collector.insert(self); + } + else if(collector.erase(self)) + delete self; + } else { + int nc = unwrap(in[1]); + if(nc == 0) { + self = new Shared(new Point2()); + } + if(nc == 1) { + double x = unwrap< double >(in[2]); + double y = unwrap< double >(in[3]); + self = new Shared(new Point2(x,y)); + } + collector.insert(self); + std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetPr(out[0])) = self; + } } diff --git a/wrap/tests/expected/new_Point2_.m b/wrap/tests/expected/new_Point2_.m index b0b655b5e..b5fdcff15 100644 --- a/wrap/tests/expected/new_Point2_.m +++ b/wrap/tests/expected/new_Point2_.m @@ -1,4 +1,4 @@ % automatically generated by wrap -function result = new_Point2_(obj) +function result = new_Point2_(obj,x,y) error('need to compile new_Point2_.cpp'); end diff --git a/wrap/tests/expected/new_Point2_dd.cpp b/wrap/tests/expected/new_Point2_dd.cpp deleted file mode 100644 index 7c7f062b7..000000000 --- a/wrap/tests/expected/new_Point2_dd.cpp +++ /dev/null @@ -1,11 +0,0 @@ -// automatically generated by wrap -#include -#include -void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("new_Point2_dd",nargout,nargin,2); - double x = unwrap< double >(in[0]); - double y = unwrap< double >(in[1]); - Point2* self = new Point2(x,y); - out[0] = wrap_constructed(self,"Point2"); -} diff --git a/wrap/tests/expected/new_Point2_dd.m b/wrap/tests/expected/new_Point2_dd.m deleted file mode 100644 index 4a769ce30..000000000 --- a/wrap/tests/expected/new_Point2_dd.m +++ /dev/null @@ -1,4 +0,0 @@ -% automatically generated by wrap -function result = new_Point2_dd(obj,x,y) - error('need to compile new_Point2_dd.cpp'); -end diff --git a/wrap/tests/expected/new_Point3_.cpp b/wrap/tests/expected/new_Point3_.cpp new file mode 100644 index 000000000..368b3069e --- /dev/null +++ b/wrap/tests/expected/new_Point3_.cpp @@ -0,0 +1,47 @@ +// automatically generated by wrap +#include +#include +#include +using namespace geometry; +typedef boost::shared_ptr Shared; + +static std::set collector; + +struct Destruct +{ + void operator() (Shared* p) + { + collector.erase(p); + } +}; + +void cleanup(void) { + std::for_each( collector.begin(), collector.end(), Destruct() ); +} +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(cleanup); + + const mxArray* input = in[0]; + Shared* self = *(Shared**) mxGetData(input); + + if(self) { + if(nargin > 1) { + collector.insert(self); + } + else if(collector.erase(self)) + delete self; + } else { + int nc = unwrap(in[1]); + if(nc == 0) { + double x = unwrap< double >(in[2]); + double y = unwrap< double >(in[3]); + double z = unwrap< double >(in[4]); + self = new Shared(new Point3(x,y,z)); + } + collector.insert(self); + std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetPr(out[0])) = self; + } +} diff --git a/wrap/tests/expected/new_Point3_.m b/wrap/tests/expected/new_Point3_.m new file mode 100644 index 000000000..f306af227 --- /dev/null +++ b/wrap/tests/expected/new_Point3_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap +function result = new_Point3_(obj,x,y,z) + error('need to compile new_Point3_.cpp'); +end diff --git a/wrap/tests/expected/new_Point3_ddd.cpp b/wrap/tests/expected/new_Point3_ddd.cpp deleted file mode 100644 index aa45dc71c..000000000 --- a/wrap/tests/expected/new_Point3_ddd.cpp +++ /dev/null @@ -1,13 +0,0 @@ -// automatically generated by wrap -#include -#include -using namespace geometry; -void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("new_Point3_ddd",nargout,nargin,3); - double x = unwrap< double >(in[0]); - double y = unwrap< double >(in[1]); - double z = unwrap< double >(in[2]); - Point3* self = new Point3(x,y,z); - out[0] = wrap_constructed(self,"Point3"); -} diff --git a/wrap/tests/expected/new_Point3_ddd.m b/wrap/tests/expected/new_Point3_ddd.m deleted file mode 100644 index 154dda1d0..000000000 --- a/wrap/tests/expected/new_Point3_ddd.m +++ /dev/null @@ -1,4 +0,0 @@ -% automatically generated by wrap -function result = new_Point3_ddd(obj,x,y,z) - error('need to compile new_Point3_ddd.cpp'); -end diff --git a/wrap/tests/expected/new_Test_.cpp b/wrap/tests/expected/new_Test_.cpp index 9c66706ae..a6bcc9549 100644 --- a/wrap/tests/expected/new_Test_.cpp +++ b/wrap/tests/expected/new_Test_.cpp @@ -1,10 +1,49 @@ // automatically generated by wrap #include +#include #include using namespace geometry; +typedef boost::shared_ptr Shared; + +static std::set collector; + +struct Destruct +{ + void operator() (Shared* p) + { + collector.erase(p); + } +}; + +void cleanup(void) { + std::for_each( collector.begin(), collector.end(), Destruct() ); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("new_Test_",nargout,nargin,0); - Test* self = new Test(); - out[0] = wrap_constructed(self,"Test"); + mexAtExit(cleanup); + + const mxArray* input = in[0]; + Shared* self = *(Shared**) mxGetData(input); + + if(self) { + if(nargin > 1) { + collector.insert(self); + } + else if(collector.erase(self)) + delete self; + } else { + int nc = unwrap(in[1]); + if(nc == 0) { + self = new Shared(new Test()); + } + if(nc == 1) { + double a = unwrap< double >(in[2]); + Matrix b = unwrap< Matrix >(in[3]); + self = new Shared(new Test(a,b)); + } + collector.insert(self); + std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetPr(out[0])) = self; + } } diff --git a/wrap/tests/expected/new_Test_.m b/wrap/tests/expected/new_Test_.m index 33c465f8b..82faf0fd6 100644 --- a/wrap/tests/expected/new_Test_.m +++ b/wrap/tests/expected/new_Test_.m @@ -1,4 +1,4 @@ % automatically generated by wrap -function result = new_Test_(obj) +function result = new_Test_(obj,a,b) error('need to compile new_Test_.cpp'); end diff --git a/wrap/tests/expected/new_Test_dM.cpp b/wrap/tests/expected/new_Test_dM.cpp deleted file mode 100644 index e8a7c8de1..000000000 --- a/wrap/tests/expected/new_Test_dM.cpp +++ /dev/null @@ -1,12 +0,0 @@ -// automatically generated by wrap -#include -#include -using namespace geometry; -void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("new_Test_dM",nargout,nargin,2); - double a = unwrap< double >(in[0]); - Matrix b = unwrap< Matrix >(in[1]); - Test* self = new Test(a,b); - out[0] = wrap_constructed(self,"Test"); -} diff --git a/wrap/tests/expected/new_Test_dM.m b/wrap/tests/expected/new_Test_dM.m deleted file mode 100644 index c7009d6a9..000000000 --- a/wrap/tests/expected/new_Test_dM.m +++ /dev/null @@ -1,4 +0,0 @@ -% automatically generated by wrap -function result = new_Test_dM(obj,a,b) - error('need to compile new_Test_dM.cpp'); -end diff --git a/wrap/tests/expected_namespaces/@ClassD/ClassD.m b/wrap/tests/expected_namespaces/@ClassD/ClassD.m index 9e3ddf132..2a82b6bbf 100644 --- a/wrap/tests/expected_namespaces/@ClassD/ClassD.m +++ b/wrap/tests/expected_namespaces/@ClassD/ClassD.m @@ -5,11 +5,15 @@ classdef ClassD < handle end methods function obj = ClassD(varargin) - if (nargin == 0), obj.self = new_ClassD_(); end - if nargin ~= 13 && obj.self == 0, error('ClassD constructor failed'); end + if (nargin == 0), obj.self = new_ClassD_(0,0); end + if nargin ==14, new_ClassD_(varargin{1},0); end + if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ClassD constructor failed'); end end function delete(obj) - delete_ClassD(obj); + if obj.self ~= 0 + new_ClassD_(obj.self); + obj.self = 0; + end end function display(obj), obj.print(''); end function disp(obj), obj.display; end diff --git a/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m b/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m index 806070a36..f1cc51eda 100644 --- a/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m +++ b/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m @@ -5,11 +5,15 @@ classdef ns1ClassA < handle end methods function obj = ns1ClassA(varargin) - if (nargin == 0), obj.self = new_ns1ClassA_(); end - if nargin ~= 13 && obj.self == 0, error('ns1ClassA constructor failed'); end + if (nargin == 0), obj.self = new_ns1ClassA_(0,0); end + if nargin ==14, new_ns1ClassA_(varargin{1},0); end + if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns1ClassA constructor failed'); end end function delete(obj) - delete_ns1ClassA(obj); + if obj.self ~= 0 + new_ns1ClassA_(obj.self); + obj.self = 0; + end end function display(obj), obj.print(''); end function disp(obj), obj.display; end diff --git a/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m b/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m index 13d5846ae..9403d5b7c 100644 --- a/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m +++ b/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m @@ -5,11 +5,15 @@ classdef ns1ClassB < handle end methods function obj = ns1ClassB(varargin) - if (nargin == 0), obj.self = new_ns1ClassB_(); end - if nargin ~= 13 && obj.self == 0, error('ns1ClassB constructor failed'); end + if (nargin == 0), obj.self = new_ns1ClassB_(0,0); end + if nargin ==14, new_ns1ClassB_(varargin{1},0); end + if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns1ClassB constructor failed'); end end function delete(obj) - delete_ns1ClassB(obj); + if obj.self ~= 0 + new_ns1ClassB_(obj.self); + obj.self = 0; + end end function display(obj), obj.print(''); end function disp(obj), obj.display; end diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp index 514e5db08..668980943 100644 --- a/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp +++ b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp @@ -1,11 +1,15 @@ // automatically generated by wrap #include +#include #include #include +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("memberFunction",nargout,nargin-1,0); - boost::shared_ptr self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); - double result = self->memberFunction(); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + double result = obj->memberFunction(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m b/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m index 188ac087d..b4c9de568 100644 --- a/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m +++ b/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m @@ -5,11 +5,15 @@ classdef ns2ClassA < handle end methods function obj = ns2ClassA(varargin) - if (nargin == 0), obj.self = new_ns2ClassA_(); end - if nargin ~= 13 && obj.self == 0, error('ns2ClassA constructor failed'); end + if (nargin == 0), obj.self = new_ns2ClassA_(0,0); end + if nargin ==14, new_ns2ClassA_(varargin{1},0); end + if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns2ClassA constructor failed'); end end function delete(obj) - delete_ns2ClassA(obj); + if obj.self ~= 0 + new_ns2ClassA_(obj.self); + obj.self = 0; + end end function display(obj), obj.print(''); end function disp(obj), obj.display; end diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/nsArg.cpp b/wrap/tests/expected_namespaces/@ns2ClassA/nsArg.cpp index 789b0815e..a55fd5581 100644 --- a/wrap/tests/expected_namespaces/@ns2ClassA/nsArg.cpp +++ b/wrap/tests/expected_namespaces/@ns2ClassA/nsArg.cpp @@ -1,12 +1,16 @@ // automatically generated by wrap #include +#include #include #include +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("nsArg",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); - ns1::ClassB& arg = *unwrap_shared_ptr< ns1::ClassB >(in[1], "ns1ClassB"); - int result = self->nsArg(arg); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + ns1::ClassB& arg = *unwrap_shared_ptr< ns1::ClassB >(in[1], "ns1ClassB"); + int result = obj->nsArg(arg); out[0] = wrap< int >(result); } diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp b/wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp index dbeb42f60..334bc9928 100644 --- a/wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp +++ b/wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp @@ -1,12 +1,18 @@ // automatically generated by wrap #include +#include #include #include +typedef boost::shared_ptr SharedClassB; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("nsReturn",nargout,nargin-1,1); - boost::shared_ptr self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); - double q = unwrap< double >(in[1]); - ns2::ns3::ClassB result = self->nsReturn(q); - out[0] = wrap_shared_ptr(boost::make_shared< ns2::ns3::ClassB >(result),"ns2ns3ClassB"); + mxArray* mxh = mxGetProperty(in[0],0,"self"); + Shared* self = *reinterpret_cast (mxGetPr(mxh)); + Shared obj = *self; + double q = unwrap< double >(in[1]); + ns2::ns3::ClassB result = obj->nsReturn(q); + SharedClassB* ret = new SharedClassB(new ns2::ns3::ClassB(result)); + out[0] = wrap_collect_shared_ptr(ret,"ns2ns3ClassB"); } diff --git a/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m b/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m index 793e73e64..13fbb49f3 100644 --- a/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m +++ b/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m @@ -5,11 +5,15 @@ classdef ns2ClassC < handle end methods function obj = ns2ClassC(varargin) - if (nargin == 0), obj.self = new_ns2ClassC_(); end - if nargin ~= 13 && obj.self == 0, error('ns2ClassC constructor failed'); end + if (nargin == 0), obj.self = new_ns2ClassC_(0,0); end + if nargin ==14, new_ns2ClassC_(varargin{1},0); end + if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns2ClassC constructor failed'); end end function delete(obj) - delete_ns2ClassC(obj); + if obj.self ~= 0 + new_ns2ClassC_(obj.self); + obj.self = 0; + end end function display(obj), obj.print(''); end function disp(obj), obj.display; end diff --git a/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m b/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m index 3997cd005..29e0721f0 100644 --- a/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m +++ b/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m @@ -5,11 +5,15 @@ classdef ns2ns3ClassB < handle end methods function obj = ns2ns3ClassB(varargin) - if (nargin == 0), obj.self = new_ns2ns3ClassB_(); end - if nargin ~= 13 && obj.self == 0, error('ns2ns3ClassB constructor failed'); end + if (nargin == 0), obj.self = new_ns2ns3ClassB_(0,0); end + if nargin ==14, new_ns2ns3ClassB_(varargin{1},0); end + if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns2ns3ClassB constructor failed'); end end function delete(obj) - delete_ns2ns3ClassB(obj); + if obj.self ~= 0 + new_ns2ns3ClassB_(obj.self); + obj.self = 0; + end end function display(obj), obj.print(''); end function disp(obj), obj.display; end diff --git a/wrap/tests/expected_namespaces/Makefile b/wrap/tests/expected_namespaces/Makefile index c48a1b154..dcc3b1dbd 100644 --- a/wrap/tests/expected_namespaces/Makefile +++ b/wrap/tests/expected_namespaces/Makefile @@ -9,24 +9,18 @@ all: ns1ClassA ns1ClassB ns2ClassA ns2ns3ClassB ns2ClassC ClassD # ns1ClassA new_ns1ClassA_.$(MEXENDING): new_ns1ClassA_.cpp $(MEX) $(mex_flags) new_ns1ClassA_.cpp -output new_ns1ClassA_ -delete_ns1ClassA.$(MEXENDING): delete_ns1ClassA.cpp - $(MEX) $(mex_flags) delete_ns1ClassA.cpp -output delete_ns1ClassA -ns1ClassA: new_ns1ClassA_.$(MEXENDING) delete_ns1ClassA.$(MEXENDING) +ns1ClassA: new_ns1ClassA_.$(MEXENDING) # ns1ClassB new_ns1ClassB_.$(MEXENDING): new_ns1ClassB_.cpp $(MEX) $(mex_flags) new_ns1ClassB_.cpp -output new_ns1ClassB_ -delete_ns1ClassB.$(MEXENDING): delete_ns1ClassB.cpp - $(MEX) $(mex_flags) delete_ns1ClassB.cpp -output delete_ns1ClassB -ns1ClassB: new_ns1ClassB_.$(MEXENDING) delete_ns1ClassB.$(MEXENDING) +ns1ClassB: new_ns1ClassB_.$(MEXENDING) # ns2ClassA new_ns2ClassA_.$(MEXENDING): new_ns2ClassA_.cpp $(MEX) $(mex_flags) new_ns2ClassA_.cpp -output new_ns2ClassA_ -delete_ns2ClassA.$(MEXENDING): delete_ns2ClassA.cpp - $(MEX) $(mex_flags) delete_ns2ClassA.cpp -output delete_ns2ClassA ns2ClassA_afunction.$(MEXENDING): ns2ClassA_afunction.cpp $(MEX) $(mex_flags) ns2ClassA_afunction.cpp -output ns2ClassA_afunction @ns2ClassA/memberFunction.$(MEXENDING): @ns2ClassA/memberFunction.cpp @@ -36,31 +30,25 @@ ns2ClassA_afunction.$(MEXENDING): ns2ClassA_afunction.cpp @ns2ClassA/nsReturn.$(MEXENDING): @ns2ClassA/nsReturn.cpp $(MEX) $(mex_flags) @ns2ClassA/nsReturn.cpp -output @ns2ClassA/nsReturn -ns2ClassA: new_ns2ClassA_.$(MEXENDING) delete_ns2ClassA.$(MEXENDING) ns2ClassA_afunction.$(MEXENDING) @ns2ClassA/memberFunction.$(MEXENDING) @ns2ClassA/nsArg.$(MEXENDING) @ns2ClassA/nsReturn.$(MEXENDING) +ns2ClassA: new_ns2ClassA_.$(MEXENDING) ns2ClassA_afunction.$(MEXENDING) @ns2ClassA/memberFunction.$(MEXENDING) @ns2ClassA/nsArg.$(MEXENDING) @ns2ClassA/nsReturn.$(MEXENDING) # ns2ns3ClassB new_ns2ns3ClassB_.$(MEXENDING): new_ns2ns3ClassB_.cpp $(MEX) $(mex_flags) new_ns2ns3ClassB_.cpp -output new_ns2ns3ClassB_ -delete_ns2ns3ClassB.$(MEXENDING): delete_ns2ns3ClassB.cpp - $(MEX) $(mex_flags) delete_ns2ns3ClassB.cpp -output delete_ns2ns3ClassB -ns2ns3ClassB: new_ns2ns3ClassB_.$(MEXENDING) delete_ns2ns3ClassB.$(MEXENDING) +ns2ns3ClassB: new_ns2ns3ClassB_.$(MEXENDING) # ns2ClassC new_ns2ClassC_.$(MEXENDING): new_ns2ClassC_.cpp $(MEX) $(mex_flags) new_ns2ClassC_.cpp -output new_ns2ClassC_ -delete_ns2ClassC.$(MEXENDING): delete_ns2ClassC.cpp - $(MEX) $(mex_flags) delete_ns2ClassC.cpp -output delete_ns2ClassC -ns2ClassC: new_ns2ClassC_.$(MEXENDING) delete_ns2ClassC.$(MEXENDING) +ns2ClassC: new_ns2ClassC_.$(MEXENDING) # ClassD new_ClassD_.$(MEXENDING): new_ClassD_.cpp $(MEX) $(mex_flags) new_ClassD_.cpp -output new_ClassD_ -delete_ClassD.$(MEXENDING): delete_ClassD.cpp - $(MEX) $(mex_flags) delete_ClassD.cpp -output delete_ClassD -ClassD: new_ClassD_.$(MEXENDING) delete_ClassD.$(MEXENDING) +ClassD: new_ClassD_.$(MEXENDING) diff --git a/wrap/tests/expected_namespaces/delete_ClassD.cpp b/wrap/tests/expected_namespaces/delete_ClassD.cpp deleted file mode 100644 index 5284e33ba..000000000 --- a/wrap/tests/expected_namespaces/delete_ClassD.cpp +++ /dev/null @@ -1,8 +0,0 @@ -// automatically generated by wrap -#include -#include -void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("delete_ClassD",nargout,nargin,1); - delete_shared_ptr< ClassD >(in[0],"ClassD"); -} diff --git a/wrap/tests/expected_namespaces/delete_ClassD.m b/wrap/tests/expected_namespaces/delete_ClassD.m deleted file mode 100644 index aef8cb642..000000000 --- a/wrap/tests/expected_namespaces/delete_ClassD.m +++ /dev/null @@ -1,4 +0,0 @@ -% automatically generated by wrap -function result = delete_ClassD(obj) - error('need to compile delete_ClassD.cpp'); -end diff --git a/wrap/tests/expected_namespaces/delete_ns1ClassA.cpp b/wrap/tests/expected_namespaces/delete_ns1ClassA.cpp deleted file mode 100644 index 17a25b523..000000000 --- a/wrap/tests/expected_namespaces/delete_ns1ClassA.cpp +++ /dev/null @@ -1,8 +0,0 @@ -// automatically generated by wrap -#include -#include -void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("delete_ns1ClassA",nargout,nargin,1); - delete_shared_ptr< ns1::ClassA >(in[0],"ns1ClassA"); -} diff --git a/wrap/tests/expected_namespaces/delete_ns1ClassA.m b/wrap/tests/expected_namespaces/delete_ns1ClassA.m deleted file mode 100644 index 343d5636a..000000000 --- a/wrap/tests/expected_namespaces/delete_ns1ClassA.m +++ /dev/null @@ -1,4 +0,0 @@ -% automatically generated by wrap -function result = delete_ns1ClassA(obj) - error('need to compile delete_ns1ClassA.cpp'); -end diff --git a/wrap/tests/expected_namespaces/delete_ns1ClassB.cpp b/wrap/tests/expected_namespaces/delete_ns1ClassB.cpp deleted file mode 100644 index 7302cc3d3..000000000 --- a/wrap/tests/expected_namespaces/delete_ns1ClassB.cpp +++ /dev/null @@ -1,9 +0,0 @@ -// automatically generated by wrap -#include -#include -#include -void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("delete_ns1ClassB",nargout,nargin,1); - delete_shared_ptr< ns1::ClassB >(in[0],"ns1ClassB"); -} diff --git a/wrap/tests/expected_namespaces/delete_ns1ClassB.m b/wrap/tests/expected_namespaces/delete_ns1ClassB.m deleted file mode 100644 index ec50f3f06..000000000 --- a/wrap/tests/expected_namespaces/delete_ns1ClassB.m +++ /dev/null @@ -1,4 +0,0 @@ -% automatically generated by wrap -function result = delete_ns1ClassB(obj) - error('need to compile delete_ns1ClassB.cpp'); -end diff --git a/wrap/tests/expected_namespaces/delete_ns2ClassA.cpp b/wrap/tests/expected_namespaces/delete_ns2ClassA.cpp deleted file mode 100644 index 0562ee073..000000000 --- a/wrap/tests/expected_namespaces/delete_ns2ClassA.cpp +++ /dev/null @@ -1,9 +0,0 @@ -// automatically generated by wrap -#include -#include -#include -void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("delete_ns2ClassA",nargout,nargin,1); - delete_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); -} diff --git a/wrap/tests/expected_namespaces/delete_ns2ClassA.m b/wrap/tests/expected_namespaces/delete_ns2ClassA.m deleted file mode 100644 index 4f1b92aa5..000000000 --- a/wrap/tests/expected_namespaces/delete_ns2ClassA.m +++ /dev/null @@ -1,4 +0,0 @@ -% automatically generated by wrap -function result = delete_ns2ClassA(obj) - error('need to compile delete_ns2ClassA.cpp'); -end diff --git a/wrap/tests/expected_namespaces/delete_ns2ClassC.cpp b/wrap/tests/expected_namespaces/delete_ns2ClassC.cpp deleted file mode 100644 index ef57796b7..000000000 --- a/wrap/tests/expected_namespaces/delete_ns2ClassC.cpp +++ /dev/null @@ -1,8 +0,0 @@ -// automatically generated by wrap -#include -#include -void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("delete_ns2ClassC",nargout,nargin,1); - delete_shared_ptr< ns2::ClassC >(in[0],"ns2ClassC"); -} diff --git a/wrap/tests/expected_namespaces/delete_ns2ClassC.m b/wrap/tests/expected_namespaces/delete_ns2ClassC.m deleted file mode 100644 index 1db1ddc93..000000000 --- a/wrap/tests/expected_namespaces/delete_ns2ClassC.m +++ /dev/null @@ -1,4 +0,0 @@ -% automatically generated by wrap -function result = delete_ns2ClassC(obj) - error('need to compile delete_ns2ClassC.cpp'); -end diff --git a/wrap/tests/expected_namespaces/delete_ns2ns3ClassB.cpp b/wrap/tests/expected_namespaces/delete_ns2ns3ClassB.cpp deleted file mode 100644 index 0a6c4ce73..000000000 --- a/wrap/tests/expected_namespaces/delete_ns2ns3ClassB.cpp +++ /dev/null @@ -1,9 +0,0 @@ -// automatically generated by wrap -#include -#include -#include -void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) -{ - checkArguments("delete_ns2ns3ClassB",nargout,nargin,1); - delete_shared_ptr< ns2::ns3::ClassB >(in[0],"ns2ns3ClassB"); -} diff --git a/wrap/tests/expected_namespaces/delete_ns2ns3ClassB.m b/wrap/tests/expected_namespaces/delete_ns2ns3ClassB.m deleted file mode 100644 index 427359eca..000000000 --- a/wrap/tests/expected_namespaces/delete_ns2ns3ClassB.m +++ /dev/null @@ -1,4 +0,0 @@ -% automatically generated by wrap -function result = delete_ns2ns3ClassB(obj) - error('need to compile delete_ns2ns3ClassB.cpp'); -end diff --git a/wrap/tests/expected_namespaces/make_testNamespaces.m b/wrap/tests/expected_namespaces/make_testNamespaces.m index d835a2da0..5ff13e5d4 100644 --- a/wrap/tests/expected_namespaces/make_testNamespaces.m +++ b/wrap/tests/expected_namespaces/make_testNamespaces.m @@ -10,21 +10,18 @@ addpath(toolboxpath); %% ns1ClassA cd(toolboxpath) mex -O5 new_ns1ClassA_.cpp -mex -O5 delete_ns1ClassA.cpp cd @ns1ClassA %% ns1ClassB cd(toolboxpath) mex -O5 new_ns1ClassB_.cpp -mex -O5 delete_ns1ClassB.cpp cd @ns1ClassB %% ns2ClassA cd(toolboxpath) mex -O5 new_ns2ClassA_.cpp -mex -O5 delete_ns2ClassA.cpp mex -O5 ns2ClassA_afunction.cpp cd @ns2ClassA @@ -35,21 +32,18 @@ mex -O5 nsReturn.cpp %% ns2ns3ClassB cd(toolboxpath) mex -O5 new_ns2ns3ClassB_.cpp -mex -O5 delete_ns2ns3ClassB.cpp cd @ns2ns3ClassB %% ns2ClassC cd(toolboxpath) mex -O5 new_ns2ClassC_.cpp -mex -O5 delete_ns2ClassC.cpp cd @ns2ClassC %% ClassD cd(toolboxpath) mex -O5 new_ClassD_.cpp -mex -O5 delete_ClassD.cpp cd @ClassD diff --git a/wrap/tests/expected_namespaces/new_ClassD_.cpp b/wrap/tests/expected_namespaces/new_ClassD_.cpp index 59ea4398b..352c470b0 100644 --- a/wrap/tests/expected_namespaces/new_ClassD_.cpp +++ b/wrap/tests/expected_namespaces/new_ClassD_.cpp @@ -1,9 +1,43 @@ // automatically generated by wrap #include +#include #include +typedef boost::shared_ptr Shared; + +static std::set collector; + +struct Destruct +{ + void operator() (Shared* p) + { + collector.erase(p); + } +}; + +void cleanup(void) { + std::for_each( collector.begin(), collector.end(), Destruct() ); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("new_ClassD_",nargout,nargin,0); - ClassD* self = new ClassD(); - out[0] = wrap_constructed(self,"ClassD"); + mexAtExit(cleanup); + + const mxArray* input = in[0]; + Shared* self = *(Shared**) mxGetData(input); + + if(self) { + if(nargin > 1) { + collector.insert(self); + } + else if(collector.erase(self)) + delete self; + } else { + int nc = unwrap(in[1]); + if(nc == 0) { + self = new Shared(new ClassD()); + } + collector.insert(self); + std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetPr(out[0])) = self; + } } diff --git a/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp b/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp index cbb82eb70..893331034 100644 --- a/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp +++ b/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp @@ -1,9 +1,43 @@ // automatically generated by wrap #include +#include #include +typedef boost::shared_ptr Shared; + +static std::set collector; + +struct Destruct +{ + void operator() (Shared* p) + { + collector.erase(p); + } +}; + +void cleanup(void) { + std::for_each( collector.begin(), collector.end(), Destruct() ); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("new_ns1ClassA_",nargout,nargin,0); - ns1::ClassA* self = new ns1::ClassA(); - out[0] = wrap_constructed(self,"ns1ClassA"); + mexAtExit(cleanup); + + const mxArray* input = in[0]; + Shared* self = *(Shared**) mxGetData(input); + + if(self) { + if(nargin > 1) { + collector.insert(self); + } + else if(collector.erase(self)) + delete self; + } else { + int nc = unwrap(in[1]); + if(nc == 0) { + self = new Shared(new ns1::ClassA()); + } + collector.insert(self); + std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetPr(out[0])) = self; + } } diff --git a/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp b/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp index d50cede19..047348d1b 100644 --- a/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp +++ b/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp @@ -1,10 +1,44 @@ // automatically generated by wrap #include +#include #include #include +typedef boost::shared_ptr Shared; + +static std::set collector; + +struct Destruct +{ + void operator() (Shared* p) + { + collector.erase(p); + } +}; + +void cleanup(void) { + std::for_each( collector.begin(), collector.end(), Destruct() ); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("new_ns1ClassB_",nargout,nargin,0); - ns1::ClassB* self = new ns1::ClassB(); - out[0] = wrap_constructed(self,"ns1ClassB"); + mexAtExit(cleanup); + + const mxArray* input = in[0]; + Shared* self = *(Shared**) mxGetData(input); + + if(self) { + if(nargin > 1) { + collector.insert(self); + } + else if(collector.erase(self)) + delete self; + } else { + int nc = unwrap(in[1]); + if(nc == 0) { + self = new Shared(new ns1::ClassB()); + } + collector.insert(self); + std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetPr(out[0])) = self; + } } diff --git a/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp b/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp index 81bae3198..cb06bf892 100644 --- a/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp +++ b/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp @@ -1,10 +1,44 @@ // automatically generated by wrap #include +#include #include #include +typedef boost::shared_ptr Shared; + +static std::set collector; + +struct Destruct +{ + void operator() (Shared* p) + { + collector.erase(p); + } +}; + +void cleanup(void) { + std::for_each( collector.begin(), collector.end(), Destruct() ); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("new_ns2ClassA_",nargout,nargin,0); - ns2::ClassA* self = new ns2::ClassA(); - out[0] = wrap_constructed(self,"ns2ClassA"); + mexAtExit(cleanup); + + const mxArray* input = in[0]; + Shared* self = *(Shared**) mxGetData(input); + + if(self) { + if(nargin > 1) { + collector.insert(self); + } + else if(collector.erase(self)) + delete self; + } else { + int nc = unwrap(in[1]); + if(nc == 0) { + self = new Shared(new ns2::ClassA()); + } + collector.insert(self); + std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetPr(out[0])) = self; + } } diff --git a/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp b/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp index fbee07926..40c3343ef 100644 --- a/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp +++ b/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp @@ -1,9 +1,43 @@ // automatically generated by wrap #include +#include #include +typedef boost::shared_ptr Shared; + +static std::set collector; + +struct Destruct +{ + void operator() (Shared* p) + { + collector.erase(p); + } +}; + +void cleanup(void) { + std::for_each( collector.begin(), collector.end(), Destruct() ); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("new_ns2ClassC_",nargout,nargin,0); - ns2::ClassC* self = new ns2::ClassC(); - out[0] = wrap_constructed(self,"ns2ClassC"); + mexAtExit(cleanup); + + const mxArray* input = in[0]; + Shared* self = *(Shared**) mxGetData(input); + + if(self) { + if(nargin > 1) { + collector.insert(self); + } + else if(collector.erase(self)) + delete self; + } else { + int nc = unwrap(in[1]); + if(nc == 0) { + self = new Shared(new ns2::ClassC()); + } + collector.insert(self); + std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetPr(out[0])) = self; + } } diff --git a/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp index ca9d02df3..ace0a261f 100644 --- a/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp +++ b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp @@ -1,10 +1,44 @@ // automatically generated by wrap #include +#include #include #include +typedef boost::shared_ptr Shared; + +static std::set collector; + +struct Destruct +{ + void operator() (Shared* p) + { + collector.erase(p); + } +}; + +void cleanup(void) { + std::for_each( collector.begin(), collector.end(), Destruct() ); +} void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("new_ns2ns3ClassB_",nargout,nargin,0); - ns2::ns3::ClassB* self = new ns2::ns3::ClassB(); - out[0] = wrap_constructed(self,"ns2ns3ClassB"); + mexAtExit(cleanup); + + const mxArray* input = in[0]; + Shared* self = *(Shared**) mxGetData(input); + + if(self) { + if(nargin > 1) { + collector.insert(self); + } + else if(collector.erase(self)) + delete self; + } else { + int nc = unwrap(in[1]); + if(nc == 0) { + self = new Shared(new ns2::ns3::ClassB()); + } + collector.insert(self); + std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetPr(out[0])) = self; + } } diff --git a/wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp b/wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp index 72559acf8..3e8d54a14 100644 --- a/wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp +++ b/wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp @@ -1,7 +1,10 @@ // automatically generated by wrap #include +#include #include #include +typedef boost::shared_ptr Shareddouble; +typedef boost::shared_ptr Shared; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("ns2ClassA_afunction",nargout,nargin,0); diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index db3ac8982..cdc6e42d6 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -83,7 +83,7 @@ TEST( wrap, parse ) { { Class cls = module.classes.at(0); EXPECT(assert_equal("Point2", cls.name)); - EXPECT_LONGS_EQUAL(2, cls.constructors.size()); + EXPECT_LONGS_EQUAL(2, cls.constructor.args_list.size()); EXPECT_LONGS_EQUAL(7, cls.methods.size()); EXPECT_LONGS_EQUAL(0, cls.static_methods.size()); EXPECT_LONGS_EQUAL(0, cls.namespaces.size()); @@ -94,18 +94,18 @@ TEST( wrap, parse ) { { Class cls = module.classes.at(1); EXPECT(assert_equal("Point3", cls.name)); - EXPECT_LONGS_EQUAL(1, cls.constructors.size()); + EXPECT_LONGS_EQUAL(1, cls.constructor.args_list.size()); EXPECT_LONGS_EQUAL(1, cls.methods.size()); EXPECT_LONGS_EQUAL(2, cls.static_methods.size()); EXPECT_LONGS_EQUAL(0, cls.namespaces.size()); EXPECT(assert_equal(exp_using2, cls.using_namespaces)); // first constructor takes 3 doubles - Constructor c1 = cls.constructors.front(); - EXPECT_LONGS_EQUAL(3, c1.args.size()); + ArgumentList c1 = cls.constructor.args_list.front(); + EXPECT_LONGS_EQUAL(3, c1.size()); // check first double argument - Argument a1 = c1.args.front(); + Argument a1 = c1.front(); EXPECT(!a1.is_const); EXPECT(assert_equal("double", a1.type)); EXPECT(!a1.is_ref); @@ -123,7 +123,7 @@ TEST( wrap, parse ) { { LONGS_EQUAL(3, module.classes.size()); Class testCls = module.classes.at(2); - EXPECT_LONGS_EQUAL( 2, testCls.constructors.size()); + EXPECT_LONGS_EQUAL( 2, testCls.constructor.args_list.size()); EXPECT_LONGS_EQUAL(19, testCls.methods.size()); EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); EXPECT_LONGS_EQUAL( 0, testCls.namespaces.size()); @@ -228,18 +228,6 @@ TEST( wrap, matlab_code_namespaces ) { EXPECT(files_equal(exp_path + "new_ns2ClassC_.m" , act_path + "new_ns2ClassC_.m" )); EXPECT(files_equal(exp_path + "new_ns2ns3ClassB_.cpp" , act_path + "new_ns2ns3ClassB_.cpp" )); EXPECT(files_equal(exp_path + "new_ns2ns3ClassB_.m" , act_path + "new_ns2ns3ClassB_.m" )); - EXPECT(files_equal(exp_path + "delete_ClassD.cpp" , act_path + "delete_ClassD.cpp" )); - EXPECT(files_equal(exp_path + "delete_ClassD.m" , act_path + "delete_ClassD.m" )); - EXPECT(files_equal(exp_path + "delete_ns1ClassA.cpp" , act_path + "delete_ns1ClassA.cpp" )); - EXPECT(files_equal(exp_path + "delete_ns1ClassA.m" , act_path + "delete_ns1ClassA.m" )); - EXPECT(files_equal(exp_path + "delete_ns1ClassB.cpp" , act_path + "delete_ns1ClassB.cpp" )); - EXPECT(files_equal(exp_path + "delete_ns1ClassB.m" , act_path + "delete_ns1ClassB.m" )); - EXPECT(files_equal(exp_path + "delete_ns2ClassA.cpp" , act_path + "delete_ns2ClassA.cpp" )); - EXPECT(files_equal(exp_path + "delete_ns2ClassA.m" , act_path + "delete_ns2ClassA.m" )); - EXPECT(files_equal(exp_path + "delete_ns2ClassC.cpp" , act_path + "delete_ns2ClassC.cpp" )); - EXPECT(files_equal(exp_path + "delete_ns2ClassC.m" , act_path + "delete_ns2ClassC.m" )); - EXPECT(files_equal(exp_path + "delete_ns2ns3ClassB.cpp" , act_path + "delete_ns2ns3ClassB.cpp" )); - EXPECT(files_equal(exp_path + "delete_ns2ns3ClassB.m" , act_path + "delete_ns2ns3ClassB.m" )); EXPECT(files_equal(exp_path + "ns2ClassA_afunction.cpp" , act_path + "ns2ClassA_afunction.cpp" )); EXPECT(files_equal(exp_path + "ns2ClassA_afunction.m" , act_path + "ns2ClassA_afunction.m" )); @@ -268,34 +256,82 @@ TEST( wrap, matlab_code ) { // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make module.matlab_code("mex", "actual", "mexa64", "-O5"); + string epath = path + "/tests/expected/"; + string apath = "actual/"; - EXPECT(files_equal(path + "/tests/expected/@Point2/Point2.m" , "actual/@Point2/Point2.m" )); - EXPECT(files_equal(path + "/tests/expected/@Point2/x.cpp" , "actual/@Point2/x.cpp" )); - EXPECT(files_equal(path + "/tests/expected/@Point3/Point3.m" , "actual/@Point3/Point3.m" )); - EXPECT(files_equal(path + "/tests/expected/new_Point3_ddd.m" , "actual/new_Point3_ddd.m" )); - EXPECT(files_equal(path + "/tests/expected/new_Point3_ddd.cpp", "actual/new_Point3_ddd.cpp")); - EXPECT(files_equal(path + "/tests/expected/Point3_staticFunction.m" , "actual/Point3_staticFunction.m" )); - EXPECT(files_equal(path + "/tests/expected/Point3_staticFunction.cpp", "actual/Point3_staticFunction.cpp")); - EXPECT(files_equal(path + "/tests/expected/@Point3/norm.m" , "actual/@Point3/norm.m" )); - EXPECT(files_equal(path + "/tests/expected/@Point3/norm.cpp" , "actual/@Point3/norm.cpp" )); + EXPECT(files_equal(epath + "Makefile" , apath + "Makefile" )); + EXPECT(files_equal(epath + "make_geometry.m" , apath + "make_geometry.m" )); + EXPECT(files_equal(epath + "new_Point2_.cpp" , apath + "new_Point2_.cpp" )); + EXPECT(files_equal(epath + "new_Point2_.m" , apath + "new_Point2_.m" )); + EXPECT(files_equal(epath + "new_Point3_.cpp" , apath + "new_Point3_.cpp" )); + EXPECT(files_equal(epath + "new_Point3_.m" , apath + "new_Point3_.m" )); + EXPECT(files_equal(epath + "new_Test_.cpp" , apath + "new_Test_.cpp" )); + EXPECT(files_equal(epath + "new_Test_.m" , apath + "new_Test_.m" )); - EXPECT(files_equal(path + "/tests/expected/new_Test_.cpp" , "actual/new_Test_.cpp" )); - EXPECT(files_equal(path + "/tests/expected/delete_Test.cpp" , "actual/delete_Test.cpp" )); - EXPECT(files_equal(path + "/tests/expected/delete_Test.m" , "actual/delete_Test.m" )); - EXPECT(files_equal(path + "/tests/expected/@Test/Test.m" , "actual/@Test/Test.m" )); - EXPECT(files_equal(path + "/tests/expected/@Test/return_string.cpp" , "actual/@Test/return_string.cpp" )); - EXPECT(files_equal(path + "/tests/expected/@Test/return_pair.cpp" , "actual/@Test/return_pair.cpp" )); - EXPECT(files_equal(path + "/tests/expected/@Test/create_MixedPtrs.cpp", "actual/@Test/create_MixedPtrs.cpp")); - EXPECT(files_equal(path + "/tests/expected/@Test/return_field.cpp" , "actual/@Test/return_field.cpp" )); - EXPECT(files_equal(path + "/tests/expected/@Test/return_TestPtr.cpp", "actual/@Test/return_TestPtr.cpp")); - EXPECT(files_equal(path + "/tests/expected/@Test/return_Test.cpp" , "actual/@Test/return_Test.cpp" )); - EXPECT(files_equal(path + "/tests/expected/@Test/return_Point2Ptr.cpp", "actual/@Test/return_Point2Ptr.cpp")); - EXPECT(files_equal(path + "/tests/expected/@Test/return_ptrs.cpp" , "actual/@Test/return_ptrs.cpp" )); - EXPECT(files_equal(path + "/tests/expected/@Test/print.m" , "actual/@Test/print.m" )); - EXPECT(files_equal(path + "/tests/expected/@Test/print.cpp" , "actual/@Test/print.cpp" )); + EXPECT(files_equal(epath + "Point3_staticFunction.cpp" , apath + "Point3_staticFunction.cpp" )); + EXPECT(files_equal(epath + "Point3_staticFunction.m" , apath + "Point3_staticFunction.m" )); + EXPECT(files_equal(epath + "Point3_StaticFunctionRet.cpp" , apath + "Point3_StaticFunctionRet.cpp" )); + EXPECT(files_equal(epath + "Point3_StaticFunctionRet.m" , apath + "Point3_StaticFunctionRet.m" )); + + EXPECT(files_equal(epath + "@Point2/argChar.cpp" , apath + "@Point2/argChar.cpp" )); + EXPECT(files_equal(epath + "@Point2/argChar.m" , apath + "@Point2/argChar.m" )); + EXPECT(files_equal(epath + "@Point2/argUChar.cpp" , apath + "@Point2/argUChar.cpp" )); + EXPECT(files_equal(epath + "@Point2/argUChar.m" , apath + "@Point2/argUChar.m" )); + EXPECT(files_equal(epath + "@Point2/dim.cpp" , apath + "@Point2/dim.cpp" )); + EXPECT(files_equal(epath + "@Point2/dim.m" , apath + "@Point2/dim.m" )); + EXPECT(files_equal(epath + "@Point2/Point2.m" , apath + "@Point2/Point2.m" )); + EXPECT(files_equal(epath + "@Point2/returnChar.cpp" , apath + "@Point2/returnChar.cpp" )); + EXPECT(files_equal(epath + "@Point2/returnChar.m" , apath + "@Point2/returnChar.m" )); + EXPECT(files_equal(epath + "@Point2/vectorConfusion.cpp" , apath + "@Point2/vectorConfusion.cpp" )); + EXPECT(files_equal(epath + "@Point2/vectorConfusion.m" , apath + "@Point2/vectorConfusion.m" )); + EXPECT(files_equal(epath + "@Point2/x.cpp" , apath + "@Point2/x.cpp" )); + EXPECT(files_equal(epath + "@Point2/x.m" , apath + "@Point2/x.m" )); + EXPECT(files_equal(epath + "@Point2/y.cpp" , apath + "@Point2/y.cpp" )); + EXPECT(files_equal(epath + "@Point2/y.m" , apath + "@Point2/y.m" )); + EXPECT(files_equal(epath + "@Point3/norm.cpp" , apath + "@Point3/norm.cpp" )); + EXPECT(files_equal(epath + "@Point3/norm.m" , apath + "@Point3/norm.m" )); + EXPECT(files_equal(epath + "@Point3/Point3.m" , apath + "@Point3/Point3.m" )); + + EXPECT(files_equal(epath + "@Test/arg_EigenConstRef.cpp" , apath + "@Test/arg_EigenConstRef.cpp" )); + EXPECT(files_equal(epath + "@Test/arg_EigenConstRef.m" , apath + "@Test/arg_EigenConstRef.m" )); + EXPECT(files_equal(epath + "@Test/create_MixedPtrs.cpp" , apath + "@Test/create_MixedPtrs.cpp" )); + EXPECT(files_equal(epath + "@Test/create_MixedPtrs.m" , apath + "@Test/create_MixedPtrs.m" )); + EXPECT(files_equal(epath + "@Test/create_ptrs.cpp" , apath + "@Test/create_ptrs.cpp" )); + EXPECT(files_equal(epath + "@Test/create_ptrs.m" , apath + "@Test/create_ptrs.m" )); + EXPECT(files_equal(epath + "@Test/print.cpp" , apath + "@Test/print.cpp" )); + EXPECT(files_equal(epath + "@Test/print.m" , apath + "@Test/print.m" )); + EXPECT(files_equal(epath + "@Test/return_bool.cpp" , apath + "@Test/return_bool.cpp" )); + EXPECT(files_equal(epath + "@Test/return_bool.m" , apath + "@Test/return_bool.m" )); + EXPECT(files_equal(epath + "@Test/return_double.cpp" , apath + "@Test/return_double.cpp" )); + EXPECT(files_equal(epath + "@Test/return_double.m" , apath + "@Test/return_double.m" )); + EXPECT(files_equal(epath + "@Test/return_field.cpp" , apath + "@Test/return_field.cpp" )); + EXPECT(files_equal(epath + "@Test/return_field.m" , apath + "@Test/return_field.m" )); + EXPECT(files_equal(epath + "@Test/return_int.cpp" , apath + "@Test/return_int.cpp" )); + EXPECT(files_equal(epath + "@Test/return_int.m" , apath + "@Test/return_int.m" )); + EXPECT(files_equal(epath + "@Test/return_matrix1.cpp" , apath + "@Test/return_matrix1.cpp" )); + EXPECT(files_equal(epath + "@Test/return_matrix1.m" , apath + "@Test/return_matrix1.m" )); + EXPECT(files_equal(epath + "@Test/return_matrix2.cpp" , apath + "@Test/return_matrix2.cpp" )); + EXPECT(files_equal(epath + "@Test/return_matrix2.m" , apath + "@Test/return_matrix2.m" )); + EXPECT(files_equal(epath + "@Test/return_pair.cpp" , apath + "@Test/return_pair.cpp" )); + EXPECT(files_equal(epath + "@Test/return_pair.m" , apath + "@Test/return_pair.m" )); + EXPECT(files_equal(epath + "@Test/return_Point2Ptr.cpp" , apath + "@Test/return_Point2Ptr.cpp" )); + EXPECT(files_equal(epath + "@Test/return_Point2Ptr.m" , apath + "@Test/return_Point2Ptr.m" )); + EXPECT(files_equal(epath + "@Test/return_ptrs.cpp" , apath + "@Test/return_ptrs.cpp" )); + EXPECT(files_equal(epath + "@Test/return_ptrs.m" , apath + "@Test/return_ptrs.m" )); + EXPECT(files_equal(epath + "@Test/return_size_t.cpp" , apath + "@Test/return_size_t.cpp" )); + EXPECT(files_equal(epath + "@Test/return_size_t.m" , apath + "@Test/return_size_t.m" )); + EXPECT(files_equal(epath + "@Test/return_string.cpp" , apath + "@Test/return_string.cpp" )); + EXPECT(files_equal(epath + "@Test/return_string.m" , apath + "@Test/return_string.m" )); + EXPECT(files_equal(epath + "@Test/return_Test.cpp" , apath + "@Test/return_Test.cpp" )); + EXPECT(files_equal(epath + "@Test/return_Test.m" , apath + "@Test/return_Test.m" )); + EXPECT(files_equal(epath + "@Test/return_TestPtr.cpp" , apath + "@Test/return_TestPtr.cpp" )); + EXPECT(files_equal(epath + "@Test/return_TestPtr.m" , apath + "@Test/return_TestPtr.m" )); + EXPECT(files_equal(epath + "@Test/return_vector1.cpp" , apath + "@Test/return_vector1.cpp" )); + EXPECT(files_equal(epath + "@Test/return_vector1.m" , apath + "@Test/return_vector1.m" )); + EXPECT(files_equal(epath + "@Test/return_vector2.cpp" , apath + "@Test/return_vector2.cpp" )); + EXPECT(files_equal(epath + "@Test/return_vector2.m" , apath + "@Test/return_vector2.m" )); + EXPECT(files_equal(epath + "@Test/Test.m" , apath + "@Test/Test.m" )); - EXPECT(files_equal(path + "/tests/expected/make_geometry.m", "actual/make_geometry.m")); - EXPECT(files_equal(path + "/tests/expected/Makefile" , "actual/Makefile" )); } /* ************************************************************************* */ From b1eedba89f1a9e06a6cc49a094e80b34621a424e Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 28 Jun 2012 17:44:17 +0000 Subject: [PATCH 09/21] Added back NonlinearFactor::dim() --- gtsam.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index e4993413a..755402554 100644 --- a/gtsam.h +++ b/gtsam.h @@ -918,7 +918,7 @@ class NonlinearFactor { void equals(const gtsam::NonlinearFactor& other, double tol) const; gtsam::KeyVector keys() const; size_t size() const; -// size_t dim() const; // FIXME: Doesn't link + size_t dim() const; }; class EasyFactorGraph { From 1395a11d3178142f2af63132b334e484e9980bfc Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 28 Jun 2012 19:43:55 +0000 Subject: [PATCH 10/21] Merge branch 'master' into new_wrap_local --- gtsam/linear/GaussianFactor.h | 7 +++++++ gtsam/linear/HessianFactor.cpp | 20 ++++++++++++++++++++ gtsam/linear/HessianFactor.h | 7 +++++++ gtsam/linear/JacobianFactor.cpp | 6 ++++++ gtsam/linear/JacobianFactor.h | 7 +++++++ gtsam/slam/AntiFactor.h | 26 ++------------------------ 6 files changed, 49 insertions(+), 24 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index a20c09a43..0d435ce17 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -115,6 +115,13 @@ namespace gtsam { IndexFactor::permuteWithInverse(inversePermutation); } + /** + * Construct the corresponding anti-factor to negate information + * stored stored in this factor. + * @return a HessianFactor with negated Hessian matrices + */ + virtual GaussianFactor::shared_ptr negate() const = 0; + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 86938a9bf..cdf597411 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -511,4 +511,24 @@ GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFr return conditional; } +/* ************************************************************************* */ +GaussianFactor::shared_ptr HessianFactor::negate() const { + // Copy Hessian Blocks from Hessian factor and invert + std::vector js; + std::vector Gs; + std::vector gs; + double f; + js.insert(js.end(), begin(), end()); + for(size_t i = 0; i < js.size(); ++i){ + for(size_t j = i; j < js.size(); ++j){ + Gs.push_back( -info(begin()+i, begin()+j) ); + } + gs.push_back( -linearTerm(begin()+i) ); + } + f = -constantTerm(); + + // Create the Anti-Hessian Factor from the negated blocks + return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); +} + } // gtsam diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index f90a33c5e..74216f0f0 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -237,6 +237,13 @@ namespace gtsam { /** Return the number of columns and rows of the Hessian matrix */ size_t rows() const { return info_.rows(); } + /** + * Construct the corresponding anti-factor to negate information + * stored stored in this factor. + * @return a HessianFactor with negated Hessian matrices + */ + virtual GaussianFactor::shared_ptr negate() const; + /** Return a view of the block at (j1,j2) of the upper-triangular part of the * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation * above to explain that only the upper-triangular part of the information matrix is stored diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 038f8c718..b241ff24a 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -385,6 +385,12 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + GaussianFactor::shared_ptr JacobianFactor::negate() const { + HessianFactor hessian(*this); + return hessian.negate(); + } + /* ************************************************************************* */ GaussianConditional::shared_ptr JacobianFactor::eliminateFirst() { return this->eliminate(1); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 0ac085987..660eb25f7 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -169,6 +169,13 @@ namespace gtsam { */ virtual Matrix computeInformation() const; + /** + * Construct the corresponding anti-factor to negate information + * stored stored in this factor. + * @return a HessianFactor with negated Hessian matrices + */ + virtual GaussianFactor::shared_ptr negate() const; + /** Check if the factor contains no information, i.e. zero rows. This does * not necessarily mean that the factor involves no variables (to check for * involving no variables use keys().empty()). diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index 1e0d64db7..19f73c8b9 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -99,30 +99,8 @@ namespace gtsam { // Generate the linearized factor from the contained nonlinear factor GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c, ordering); - // Cast the GaussianFactor to a Hessian - HessianFactor::shared_ptr hessianFactor = boost::dynamic_pointer_cast(gaussianFactor); - - // If the cast fails, convert it to a Hessian - if(!hessianFactor){ - hessianFactor = HessianFactor::shared_ptr(new HessianFactor(*gaussianFactor)); - } - - // Copy Hessian Blocks from Hessian factor and invert - std::vector js; - std::vector Gs; - std::vector gs; - double f; - js.insert(js.end(), hessianFactor->begin(), hessianFactor->end()); - for(size_t i = 0; i < js.size(); ++i){ - for(size_t j = i; j < js.size(); ++j){ - Gs.push_back( -hessianFactor->info(hessianFactor->begin()+i, hessianFactor->begin()+j) ); - } - gs.push_back( -hessianFactor->linearTerm(hessianFactor->begin()+i) ); - } - f = -hessianFactor->constantTerm(); - - // Create the Anti-Hessian Factor from the negated blocks - return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); + // return the negated version of the factor + return gaussianFactor->negate(); } From 3f4446df0391b2716370df357cb7ad9cfbdd3751 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 29 Jun 2012 16:48:09 +0000 Subject: [PATCH 11/21] Merge branch 'master' into new_wrap_local --- gtsam/base/timing.cpp | 2 +- gtsam/nonlinear/ISAM2-impl.cpp | 73 ++++++++++++++++++- gtsam/nonlinear/ISAM2-impl.h | 16 ++++- gtsam/nonlinear/ISAM2.cpp | 7 +- gtsam/nonlinear/ISAM2.h | 9 ++- tests/testGaussianISAM2.cpp | 123 +++++++++++++++++++++++++++++++++ 6 files changed, 224 insertions(+), 6 deletions(-) diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index c16138cef..162a1306f 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -207,7 +207,7 @@ void toc_(size_t id) { != current->parent_.lock()->children_.end()) std::cout << "gtsam timing: Incorrect ID passed to toc, expected " << std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) - current->parent_.lock()->children_.begin() - << ", got " << id << std::endl; + << " \"" << current->label_ << "\", got " << id << std::endl; else std::cout << "gtsam timing: Incorrect ID passed to toc, id " << id << " does not exist" << std::endl; timingRoot->print(); diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 509143b0b..8b3bd4a0d 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -82,7 +82,7 @@ FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const N } /* ************************************************************************* */ -FastSet ISAM2::Impl::CheckRelinearization(const Permuted& delta, const Ordering& ordering, +FastSet ISAM2::Impl::CheckRelinearizationFull(const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { FastSet relinKeys; @@ -109,6 +109,77 @@ FastSet ISAM2::Impl::CheckRelinearization(const Permuted& d return relinKeys; } +/* ************************************************************************* */ +void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, const Permuted& delta, const ISAM2Clique::shared_ptr& clique) { + + // Check the current clique for relinearization + bool relinearize = false; + BOOST_FOREACH(Index var, clique->conditional()->keys()) { + double maxDelta = delta[var].lpNorm(); + if(maxDelta >= threshold) { + relinKeys.insert(var); + relinearize = true; + } + } + + // If this node was relinearized, also check its children + if(relinearize) { + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { + CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child); + } + } +} + +/* ************************************************************************* */ +void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const Permuted& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) { + + // Check the current clique for relinearization + bool relinearize = false; + BOOST_FOREACH(Index var, clique->conditional()->keys()) { + + // Lookup the key associated with this index + Key key = decoder.at(var); + + // Find the threshold for this variable type + const Vector& threshold = thresholds.find(Symbol(key).chr())->second; + + // Verify the threshold vector matches the actual variable size + if(threshold.rows() != delta[var].rows()) + throw std::invalid_argument("Relinearization threshold vector dimensionality passed into iSAM2 parameters does not match actual variable dimensionality"); + + // Check for relinearization + if((delta[var].array().abs() > threshold.array()).any()) { + relinKeys.insert(var); + relinearize = true; + } + } + + // If this node was relinearized, also check its children + if(relinearize) { + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { + CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, decoder, child); + } + } +} + +/* ************************************************************************* */ +FastSet ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted& delta, const Ordering& ordering, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { + + FastSet relinKeys; + + if(root) { + if(relinearizeThreshold.type() == typeid(double)) { + CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); + } else if(relinearizeThreshold.type() == typeid(FastMap)) { + Ordering::InvertedMap decoder = ordering.invert(); + CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, decoder, root); + } + } + + return relinKeys; +} + /* ************************************************************************* */ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { static const bool debug = false; diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index f2f03fb3a..0aafb3f35 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -68,7 +68,21 @@ struct ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearization(const Permuted& delta, const Ordering& ordering, + static FastSet CheckRelinearizationFull(const Permuted& delta, const Ordering& ordering, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /** + * Find the set of variables to be relinearized according to relinearizeThreshold. + * This check is performed recursively, starting at the top of the tree. Once a + * variable in the tree does not need to be relinearized, no further checks in + * that branch are performed. This is an approximation of the Full version, designed + * to save time at the expense of accuracy. + * @param delta The linear delta to check against the threshold + * @param keyFormatter Formatter for printing nonlinear keys during debugging + * @return The set of variable indices in delta whose magnitude is greater than or + * equal to relinearizeThreshold + */ + static FastSet CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index e2ab825b3..07e0f518d 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -588,8 +588,11 @@ ISAM2Result ISAM2::update( tic(5,"gather relinearize keys"); vector markedRelinMask(ordering_.nVars(), false); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); - if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging + if(params_.enablePartialRelinearizationCheck) + relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold); + else + relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, params_.relinearizeThreshold); + if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, 0.0); // This is used for debugging // Above relin threshold keys for detailed results if(params_.enableDetailedResults) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index c118cb44e..5804640df 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -125,6 +125,13 @@ struct ISAM2Params { bool enableDetailedResults; ///< Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: false) + /** Check variables for relinearization in tree-order, stopping the check once a variable does not need to be relinearized (default: false). + * This can improve speed by only checking a small part of the top of the tree. However, variables below the check cut-off can accumulate + * significant deltas without triggering relinearization. This is particularly useful in exploration scenarios where real-time performance + * is desired over correctness. Use with caution. + */ + bool enablePartialRelinearizationCheck; + /** Specify parameters as constructor arguments */ ISAM2Params( OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams @@ -139,7 +146,7 @@ struct ISAM2Params { relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization), cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter), - enableDetailedResults(false) {} + enableDetailedResults(false), enablePartialRelinearizationCheck(false) {} }; /** diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 80bd651c8..2a5bf7962 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -1242,6 +1242,129 @@ TEST(ISAM2, constrained_ordering) EXPECT(assert_equal(expectedGradient, actualGradient)); } +/* ************************************************************************* */ +TEST(ISAM2, slamlike_solution_partial_relinearization_check) +{ + + // These variables will be reused and accumulate factors and values + ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); + params.enablePartialRelinearizationCheck = true; + ISAM2 isam(params); + Values fullinit; + planarSLAM::Graph fullgraph; + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update isam + { + planarSLAM::Graph newfactors; + newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); + + isam.update(newfactors, init); + } + + CHECK(isam_check(fullgraph, fullinit, isam)); + + // Add odometry from time 0 to time 5 + for( ; i<5; ++i) { + planarSLAM::Graph newfactors; + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + } + + // Add odometry from time 5 to 6 and landmark measurement at time 5 + { + planarSLAM::Graph newfactors; + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + + isam.update(newfactors, init); + ++ i; + } + + // Add odometry from time 6 to time 10 + for( ; i<10; ++i) { + planarSLAM::Graph newfactors; + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + } + + // Add odometry from time 10 to 11 and landmark measurement at time 10 + { + planarSLAM::Graph newfactors; + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(6.9, 0.1, 0.01)); + fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); + + isam.update(newfactors, init); + ++ i; + } + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam)); + + // Check gradient at each node + typedef ISAM2::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { + // Compute expected gradient + FactorGraph jfg; + jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); + VectorValues expectedGradient(*allocateVectorValues(isam)); + gradientAtZero(jfg, expectedGradient); + // Compare with actual gradients + int variablePosition = 0; + for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { + const int dim = clique->conditional()->dim(jit); + Vector actual = clique->gradientContribution().segment(variablePosition, dim); + EXPECT(assert_equal(expectedGradient[*jit], actual)); + variablePosition += dim; + } + LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); + } + + // Check gradient + VectorValues expectedGradient(*allocateVectorValues(isam)); + gradientAtZero(FactorGraph(isam), expectedGradient); + VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); + VectorValues actualGradient(*allocateVectorValues(isam)); + gradientAtZero(isam, actualGradient); + EXPECT(assert_equal(expectedGradient2, expectedGradient)); + EXPECT(assert_equal(expectedGradient, actualGradient)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From f0c8c023a032ed8d978e79a57daea29909362aba Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Fri, 29 Jun 2012 18:38:54 +0000 Subject: [PATCH 12/21] Added small verbosity changes, verbose now defaults to false in wrap.cpp --- wrap/Constructor.cpp | 7 ++- wrap/Constructor.h | 3 +- wrap/matlab.h | 135 ------------------------------------------- wrap/wrap.cpp | 2 +- 4 files changed, 6 insertions(+), 141 deletions(-) diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index e999951b2..7785de08f 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -128,8 +128,8 @@ void Constructor::matlab_wrapper(const string& toolboxPath, file.oss << " if(self) {" << endl; file.oss << " if(nargin > 1) {" << endl; file.oss << " collector.insert(self);" << endl; - //TODO: Add verbosity flag - //file.oss << " std::cout << \"Collected\" << collector.size() << std::endl;" << endl; + if(verbose_) + file.oss << " std::cout << \"Collected\" << collector.size() << std::endl;" << endl; file.oss << " }" << endl; file.oss << " else if(collector.erase(self))" << endl; file.oss << " delete self;" << endl; @@ -148,7 +148,8 @@ void Constructor::matlab_wrapper(const string& toolboxPath, //file.oss << " self = construct(nc, in);" << endl; file.oss << " collector.insert(self);" << endl; - file.oss << " std::cout << \"constructed \" << self << \", size=\" << collector.size() << std::endl;" << endl; + if(verbose_) + file.oss << " std::cout << \"constructed \" << self << \", size=\" << collector.size() << std::endl;" << endl; file.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);" << endl; file.oss << " *reinterpret_cast (mxGetPr(out[0])) = self;" << endl; file.oss << " }" << endl; diff --git a/wrap/Constructor.h b/wrap/Constructor.h index a766905b6..467ff3b22 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -28,12 +28,11 @@ namespace wrap { struct Constructor { /// Constructor creates an empty class - Constructor(bool verbose = true) : + Constructor(bool verbose = false) : verbose_(verbose) { } // Then the instance variables are set directly by the Module constructor - // TODO:Vector of argument lists? std::vector args_list; std::string name; bool verbose_; diff --git a/wrap/matlab.h b/wrap/matlab.h index 74c0823e4..3cfebdfe7 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -376,138 +376,3 @@ boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& cla boost::shared_ptr* spp = *reinterpret_cast**> (mxGetPr(mxh)); return *spp; } - - -//***************************************************************************** -// Shared Pointer Handle -// inspired by mexhandle, but using shared_ptr -//***************************************************************************** - -template -class ObjectHandle { -private: - ObjectHandle* signature; // use 'this' as a unique object signature - const std::type_info* type; // type checking information - boost::shared_ptr t; // object pointer - -public: - // Constructor for free-store allocated objects. - // Creates shared pointer, will delete if is last one to hold pointer - ObjectHandle(T* ptr) : - type(&typeid(T)), t(ptr) { - signature = this; - mexPrintf("Created Shared Pointer use_count = %li\n", t.use_count()); - mexPrintf("Created Pointer points to %d\n", t.get()); - - } - - // Constructor for shared pointers - // Creates shared pointer, will delete if is last one to hold pointer - ObjectHandle(boost::shared_ptr shared_ptr) : - /*type(&typeid(T)),*/ t(shared_ptr) { - signature = this; - mexPrintf("Created sp from sp use_count = %li\n", t.use_count()); - mexPrintf("Created sp from sp points to %d\n", t.get()); - } - - ~ObjectHandle() { - // object is in shared_ptr, will be automatically deleted - signature = 0; // destroy signature - // std::cout << "ObjectHandle destructor" << std::endl; - } - - // Get the actual object contained by handle - boost::shared_ptr get_object() const { - return t; - } - - // Print the mexhandle for debugging - void print(const char* str) { - mexPrintf("mexhandle %s:\n", str); - mexPrintf(" signature = %d:\n", signature); - mexPrintf(" pointer = %d:\n", t.get()); - } - - // Convert ObjectHandle to a mxArray handle (to pass back from mex-function). - // Create a numeric array as handle for an ObjectHandle. - // We ASSUME we can store object pointer in the mxUINT32 element of mxArray. - mxArray* to_mex_handle() { - mxArray* handle = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - *reinterpret_cast**> (mxGetPr(handle)) = this; - return handle; - } - - string type_name() const { - return type->name(); - } - - // Convert mxArray (passed to mex-function) to an ObjectHandle. - // Import a handle from MatLab as a mxArray of UINT32. Check that - // it is actually a pointer to an ObjectHandle. - static ObjectHandle* from_mex_handle(const mxArray* handle) { - if (mxGetClassID(handle) != mxUINT32OR64_CLASS || mxIsComplex(handle) - || mxGetM(handle) != 1 || mxGetN(handle) != 1) error( - "Parameter is not an ObjectHandle type."); - - // We *assume* we can store ObjectHandle pointer in the mxUINT32 of handle - ObjectHandle* obj = *reinterpret_cast (mxGetPr(handle)); - - if (!obj) // gross check to see we don't have an invalid pointer - error("Parameter is NULL. It does not represent an ObjectHandle object."); - // TODO: change this for max-min check for pointer values - - if (obj->signature != obj) // check memory has correct signature - error("Parameter does not represent an ObjectHandle object."); - - /* - if (*(obj->type) != typeid(T)) { // check type - mexPrintf("Given: <%s>, Required: <%s>.\n", obj->type_name(), typeid(T).name()); - error("Given ObjectHandle does not represent the correct type."); - } - */ - - return obj; - } - -}; - -//***************************************************************************** -// wrapping C++ objects in a MATLAB proxy class -//***************************************************************************** - -/* - For every C++ class Class, a matlab proxy class @Class/Class.m object - is created. Its constructor will check which of the C++ constructors - needs to be called, based on nr of arguments. It then calls the - corresponding mex function new_Class_signature, which will create a - C++ object using new, and pass the pointer to wrap_constructed - (below). This creates a mexhandle and returns it to the proxy class - constructor, which assigns it to self. Matlab owns this handle now. -*/ -template -mxArray* wrap_constructed(Class* pointer, const char *classname) { - ObjectHandle* handle = new ObjectHandle(pointer); - return handle->to_mex_handle(); -} - - - -//***************************************************************************** -// unwrapping a MATLAB proxy class to a C++ object reference -//***************************************************************************** - -/* - Besides the basis types, the only other argument type allowed is a - shared pointer to a C++ object. In this case, matlab needs to pass a - proxy class object to the mex function. [unwrap_shared_ptr] extracts - the ObjectHandle from the self property, and returns a shared pointer - to the object. -*/ - - -template -void delete_shared_ptr(const mxArray* obj, const string& className) { - mxArray* mxh = mxGetProperty(obj,0,"self"); - ObjectHandle* handle = ObjectHandle::from_mex_handle(mxh); - delete handle; -} diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index eabba647e..f4490e583 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -42,7 +42,7 @@ void generate_matlab_toolbox( { // Parse interface file into class object // This recursively creates Class objects, Method objects, etc... - wrap::Module module(interfacePath, moduleName, true); + wrap::Module module(interfacePath, moduleName, false); // Then emit MATLAB code module.matlab_code(mexCommand,toolboxPath,mexExt,mexFlags); From 3c0ea754dfa2b93144051dfdcc29b9716bb7dc50 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 29 Jun 2012 19:05:49 +0000 Subject: [PATCH 13/21] make targets for cleaning wrap --- .cproject | 358 +++++++++++++++++++++++++++++++----------------------- 1 file changed, 204 insertions(+), 154 deletions(-) diff --git a/.cproject b/.cproject index 7828f8165..f52a49e73 100644 --- a/.cproject +++ b/.cproject @@ -281,6 +281,7 @@ + @@ -307,14 +308,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -341,6 +334,7 @@ make + tests/testBayesTree.run true false @@ -348,6 +342,7 @@ make + testBinaryBayesNet.run true false @@ -395,6 +390,7 @@ make + testSymbolicBayesNet.run true false @@ -402,6 +398,7 @@ make + tests/testSymbolicFactor.run true false @@ -409,6 +406,7 @@ make + testSymbolicFactorGraph.run true false @@ -424,11 +422,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -517,22 +524,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -549,6 +540,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -573,26 +580,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run + -j2 + clean true true true @@ -677,26 +684,26 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run true true true @@ -951,6 +958,7 @@ make + testGraph.run true false @@ -958,6 +966,7 @@ make + testJunctionTree.run true false @@ -965,6 +974,7 @@ make + testSymbolicBayesNetB.run true false @@ -1002,6 +1012,14 @@ true true + + make + -j5 + all + true + true + true + make -j2 @@ -1092,6 +1110,7 @@ make + testErrors.run true false @@ -1547,7 +1566,6 @@ make - testSimulated2DOriented.run true false @@ -1587,7 +1605,6 @@ make - testSimulated2D.run true false @@ -1595,7 +1612,6 @@ make - testSimulated3D.run true false @@ -1811,7 +1827,6 @@ make - tests/testGaussianISAM2 true false @@ -1833,102 +1848,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j2 @@ -2130,6 +2049,7 @@ cpack + -G DEB true false @@ -2137,6 +2057,7 @@ cpack + -G RPM true false @@ -2144,6 +2065,7 @@ cpack + -G TGZ true false @@ -2151,6 +2073,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2244,42 +2167,130 @@ true true - + make -j5 - testSpirit.run + wrap_gtsam_clean true true true - + make -j5 - testWrap.run + wrap_gtsam_unstable_clean true true true - + make -j5 - check.wrap + wrap_gtsam_distclean true true true - + make -j5 - wrap_gtsam + wrap_gtsam_unstable_distclean true true true - + make - -j5 - wrap + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2323,7 +2334,46 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap_gtsam + true + true + true + + + make + -j5 + wrap + true + true + true + - From 02df40bd734e741783d74c1edf5efc04908e8e4d Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Sat, 30 Jun 2012 01:33:41 +0000 Subject: [PATCH 14/21] Dependency checking for matlab.h --- wrap/Class.cpp | 3 ++- wrap/Module.cpp | 3 ++- wrap/Module.h | 1 + wrap/wrap.cpp | 7 ++++--- 4 files changed, 9 insertions(+), 5 deletions(-) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 5e0237d3a..e57a85884 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -152,7 +152,8 @@ void Class::makefile_fragment(FileWriter& file) const { } BOOST_FOREACH(const string& file_base, file_names) { - file.oss << file_base << ".$(MEXENDING): " << file_base << ".cpp" << endl; + file.oss << file_base << ".$(MEXENDING): " << file_base << ".cpp"; + file.oss << " $(PATH_TO_WRAP)/matlab.h" << endl; file.oss << "\t$(MEX) $(mex_flags) " << file_base << ".cpp -output " << file_base << endl; } diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 972bacbee..0adf1cda0 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -287,7 +287,7 @@ void verifyReturnTypes(const vector& validtypes, const vector& vt) { /* ************************************************************************* */ void Module::matlab_code(const string& mexCommand, const string& toolboxPath, - const string& mexExt, const string& mexFlags) const { + const string& mexExt, const string& headerPath,const string& mexFlags) const { fs::create_directories(toolboxPath); @@ -308,6 +308,7 @@ void Module::matlab_code(const string& mexCommand, const string& toolboxPath, makeModuleMakefile.oss << "\nMEX = " << mexCommand << "\n"; makeModuleMakefile.oss << "MEXENDING = " << mexExt << "\n"; + makeModuleMakefile.oss << "PATH_TO_WRAP = " << headerPath << "\n"; makeModuleMakefile.oss << "mex_flags = " << mexFlags << "\n\n"; // Dependency check list diff --git a/wrap/Module.h b/wrap/Module.h index 4b650728d..239aa20b0 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -44,6 +44,7 @@ struct Module { const std::string& mexCommand, const std::string& path, const std::string& mexExt, + const std::string& headerPath, const std::string& mexFlags) const; }; diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index f4490e583..1b1b631e4 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -38,14 +38,15 @@ void generate_matlab_toolbox( const string& interfacePath, const string& moduleName, const string& toolboxPath, - const string& mexFlags) + const string& headerPath, + const string& mexFlags) { // Parse interface file into class object // This recursively creates Class objects, Method objects, etc... wrap::Module module(interfacePath, moduleName, false); // Then emit MATLAB code - module.matlab_code(mexCommand,toolboxPath,mexExt,mexFlags); + module.matlab_code(mexCommand,toolboxPath,mexExt,headerPath,mexFlags); } /** Displays usage information */ @@ -73,6 +74,6 @@ int main(int argc, const char* argv[]) { usage(); } else - generate_matlab_toolbox(argv[1],argv[2],argv[3],argv[4],argv[5],argc==6 ? " " : argv[6]); + generate_matlab_toolbox(argv[1],argv[2],argv[3],argv[4],argv[5],argv[6],argc==7 ? " " : argv[7]); return 0; } From f68ba93f3dd47fa9613a83c212ad75a307417366 Mon Sep 17 00:00:00 2001 From: Andrew Melim Date: Sat, 30 Jun 2012 01:34:04 +0000 Subject: [PATCH 15/21] Dep checking for cmake --- CMakeLists.txt | 1 + gtsam_unstable/base/Dummy.h | 4 ++++ gtsam_unstable/gtsam_unstable.h | 1 + matlab/tests/testOdometryExample.m | 6 +++--- 4 files changed, 9 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 96efa97dd..675e7c80b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,6 +63,7 @@ option(GTSAM_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility" ON) set(GTSAM_TOOLBOX_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox") +set(GTSAM_WRAP_HEADER_PATH ${PROJECT_SOURCE_DIR}/wrap CACHE DOCSTRING "Path to directory of matlab.h") # TODO: Check for matlab mex binary before handling building of binaries diff --git a/gtsam_unstable/base/Dummy.h b/gtsam_unstable/base/Dummy.h index ccf23af5d..995186cd6 100644 --- a/gtsam_unstable/base/Dummy.h +++ b/gtsam_unstable/base/Dummy.h @@ -33,6 +33,10 @@ namespace gtsam { std::cout << s << "Dummy " << id << std::endl; } + unsigned char dummyTwoVar(unsigned char a) const { + return a; + } + }; } // namespace gtsam diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index fda6ff8c4..c9d114d58 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -14,6 +14,7 @@ namespace gtsam { class Dummy { Dummy(); void print(string s) const; + unsigned char dummyTwoVar(unsigned char a) const; }; #include diff --git a/matlab/tests/testOdometryExample.m b/matlab/tests/testOdometryExample.m index 0056a43e8..634c52799 100644 --- a/matlab/tests/testOdometryExample.m +++ b/matlab/tests/testOdometryExample.m @@ -33,8 +33,8 @@ initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1)); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd result = graph.optimize(initialEstimate,0); marginals = graph.marginals(result); -marginals.marginalCovariance(i); +marginals.marginalCovariance(1); %% Check first pose equality -pose_i = result.pose(1); -CHECK('pose_1.equals(gtsamPose2,1e-4)',pose_1.equals(gtsamPose2,1e-4)); \ No newline at end of file +pose_1 = result.pose(1); +CHECK('pose_1.equals(gtsamPose2,1e-4)',pose_1.equals(gtsamPose2,1e-4)); From 133fc4ae5cbae705ccfa785287e95df35034b187 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 30 Jun 2012 08:34:44 +0000 Subject: [PATCH 16/21] got rid of pause --- matlab/examples/OdometryExample.m | 1 - 1 file changed, 1 deletion(-) diff --git a/matlab/examples/OdometryExample.m b/matlab/examples/OdometryExample.m index 8b2439c78..b3cbff6a2 100644 --- a/matlab/examples/OdometryExample.m +++ b/matlab/examples/OdometryExample.m @@ -29,7 +29,6 @@ odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x graph.addRelativePose(1, 2, odometry, odometryNoise); graph.addRelativePose(2, 3, odometry, odometryNoise); -pause %% print graph.print(sprintf('\nFactor graph:\n')); From b33713a02ed774c31a0b0228dc92831193a286c7 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sat, 30 Jun 2012 20:04:12 +0000 Subject: [PATCH 17/21] Merge branch 'master' into new_wrap_local Conflicts: .cproject gtsam.h --- .cproject | 8 ++ gtsam.h | 2 +- gtsam/discrete/DecisionTreeFactor.cpp | 7 +- gtsam/discrete/DecisionTreeFactor.h | 3 +- gtsam/discrete/Potentials.cpp | 5 +- gtsam/discrete/Potentials.h | 3 +- gtsam/geometry/SimpleCamera.cpp | 49 ++++++++ gtsam/geometry/SimpleCamera.h | 5 + gtsam/geometry/tests/testCal3_S2.cpp | 4 +- .../tests/testSerializationGeometry.cpp | 41 ++----- gtsam/geometry/tests/testSimpleCamera.cpp | 20 ++++ gtsam/geometry/tests/testStereoCamera.cpp | 10 +- gtsam/inference/Permutation.h | 83 +------------ gtsam/inference/SymbolicFactorGraph.cpp | 4 +- gtsam/inference/VariableIndex.cpp | 52 +++----- gtsam/inference/VariableIndex.h | 50 +++----- gtsam/inference/tests/testISAM.cpp | 2 +- gtsam/linear/GaussianConditional.cpp | 36 ++---- gtsam/linear/GaussianConditional.h | 17 --- gtsam/linear/JacobianFactor.h | 2 +- gtsam/linear/NoiseModel.h | 2 + gtsam/linear/VectorValues.cpp | 40 ++++--- gtsam/linear/VectorValues.h | 13 +- .../linear/tests/testGaussianConditional.cpp | 49 -------- .../linear/tests/testGaussianJunctionTree.cpp | 2 +- .../linear/tests/testSerializationLinear.cpp | 10 +- gtsam/linear/tests/testVectorValues.cpp | 59 +++------- gtsam/nonlinear/ISAM2-impl.cpp | 67 ++++------- gtsam/nonlinear/ISAM2-impl.h | 18 +-- gtsam/nonlinear/ISAM2-inl.h | 4 +- gtsam/nonlinear/ISAM2.cpp | 30 ++--- gtsam/nonlinear/ISAM2.h | 34 ++---- gtsam/nonlinear/Values.cpp | 2 +- .../tests/testSerializationNonlinear.cpp | 12 +- gtsam/slam/tests/testStereoFactor.cpp | 8 +- gtsam_unstable/discrete/AllDiff.cpp | 5 +- gtsam_unstable/discrete/AllDiff.h | 3 +- gtsam_unstable/discrete/BinaryAllDiff.h | 7 +- gtsam_unstable/discrete/Domain.cpp | 7 +- gtsam_unstable/discrete/Domain.h | 3 +- gtsam_unstable/discrete/SingleValue.cpp | 5 +- gtsam_unstable/discrete/SingleValue.h | 3 +- .../discrete/tests/testScheduler.cpp | 2 +- tests/testGaussianISAM.cpp | 4 +- tests/testGaussianISAM2.cpp | 111 ++++++------------ tests/testGaussianJunctionTreeB.cpp | 10 +- tests/testNonlinearEquality.cpp | 14 +-- tests/testSerializationSLAM.cpp | 8 +- wrap/CMakeLists.txt | 3 +- 49 files changed, 367 insertions(+), 571 deletions(-) create mode 100644 gtsam/geometry/SimpleCamera.cpp diff --git a/.cproject b/.cproject index f52a49e73..075558734 100644 --- a/.cproject +++ b/.cproject @@ -1484,6 +1484,14 @@ true true + + make + -j5 + testSimpleCamera.run + true + true + true + make -j2 diff --git a/gtsam.h b/gtsam.h index 755402554..8b569a91e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -671,7 +671,7 @@ class VariableIndex { size_t size() const; size_t nFactors() const; size_t nEntries() const; - void permute(const gtsam::Permutation& permutation); + void permuteInPlace(const gtsam::Permutation& permutation); };*/ //************************************************************************* diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 7f566a115..bb83cec16 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -49,10 +49,11 @@ namespace gtsam { } /* ************************************************************************* */ - void DecisionTreeFactor::print(const string& s) const { + void DecisionTreeFactor::print(const string& s, + const IndexFormatter& formatter) const { cout << s; - IndexFactor::print("IndexFactor:"); - Potentials::print("Potentials:"); + IndexFactor::print("IndexFactor:",formatter); + Potentials::print("Potentials:",formatter); } /* ************************************************************************* */ diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 89bc090dc..64f37c174 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -72,7 +72,8 @@ namespace gtsam { bool equals(const DecisionTreeFactor& other, double tol = 1e-9) const; // print - void print(const std::string& s = "DecisionTreeFactor:\n") const; + virtual void print(const std::string& s = "DecisionTreeFactor:\n", + const IndexFormatter& formatter = DefaultIndexFormatter) const; /// @} /// @name Standard Interface diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index b9a87dea9..2a2260d97 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -51,10 +51,11 @@ namespace gtsam { } /* ************************************************************************* */ - void Potentials::print(const string&s) const { + void Potentials::print(const string& s, + const IndexFormatter& formatter) const { cout << s << "\n Cardinalities: "; BOOST_FOREACH(const DiscreteKey& key, cardinalities_) - cout << key.first << "=" << key.second << " "; + cout << formatter(key.first) << "=" << formatter(key.second) << " "; cout << endl; ADT::print(" "); } diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 367a323c2..3ca222b5f 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -65,7 +65,8 @@ namespace gtsam { // Testable bool equals(const Potentials& other, double tol = 1e-9) const; - void print(const std::string& s = "Potentials: ") const; + void print(const std::string& s = "Potentials: ", + const IndexFormatter& formatter = DefaultIndexFormatter) const; size_t cardinality(Index j) const { return cardinalities_.at(j);} diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp new file mode 100644 index 000000000..d441716cb --- /dev/null +++ b/gtsam/geometry/SimpleCamera.cpp @@ -0,0 +1,49 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file SimpleCamera.cpp + * @brief A simple camera class with a Cal3_S2 calibration + * @date June 30, 2012 + * @author Frank Dellaert + */ + +#include +#include + +namespace gtsam { + + SimpleCamera simpleCamera(const Matrix& P) { + + // P = [A|a] = s K cRw [I|-T], with s the unknown scale + Matrix A = P.topLeftCorner(3, 3); + Vector a = P.col(3); + + // do RQ decomposition to get s*K and cRw angles + Matrix sK; + Vector xyz; + boost::tie(sK, xyz) = RQ(A); + + // Recover scale factor s and K + double s = sK(2, 2); + Matrix K = sK / s; + + // Recover cRw itself, and its inverse + Rot3 cRw = Rot3::RzRyRx(xyz); + Rot3 wRc = cRw.inverse(); + + // Now, recover T from a = - s K cRw T = - A T + Vector T = -(A.inverse() * a); + return SimpleCamera(Pose3(wRc, T), + Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2))); + } + +} diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index d3b0cdbdc..abe42f1a0 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -22,5 +22,10 @@ #include namespace gtsam { + + /// A simple camera class with a Cal3_S2 calibration typedef PinholeCamera SimpleCamera; + + /// Recover camera from 3*4 camera matrix + SimpleCamera simpleCamera(const Matrix& P); } diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index fd82557a3..8995a3a14 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -24,8 +24,8 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2) GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2) -Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2); -Point2 p(1, -2); +static Cal3_S2 K(500, 500, 0.1, 640 / 2, 480 / 2); +static Point2 p(1, -2); /* ************************************************************************* */ TEST( Cal3_S2, easy_constructor) diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index c1fba6b2c..17a0b1eca 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -36,37 +36,20 @@ using namespace gtsam; using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ -// Export all classes derived from Value -BOOST_CLASS_EXPORT(gtsam::Cal3_S2) -BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo) -BOOST_CLASS_EXPORT(gtsam::Cal3Bundler) -BOOST_CLASS_EXPORT(gtsam::CalibratedCamera) -BOOST_CLASS_EXPORT(gtsam::Point2) -BOOST_CLASS_EXPORT(gtsam::Point3) -BOOST_CLASS_EXPORT(gtsam::Pose2) -BOOST_CLASS_EXPORT(gtsam::Pose3) -BOOST_CLASS_EXPORT(gtsam::Rot2) -BOOST_CLASS_EXPORT(gtsam::Rot3) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::PinholeCamera) -BOOST_CLASS_EXPORT(gtsam::StereoPoint2) +static Point3 pt3(1.0, 2.0, 3.0); +static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +static Pose3 pose3(rt3, pt3); -/* ************************************************************************* */ -Point3 pt3(1.0, 2.0, 3.0); -Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); -Pose3 pose3(rt3, pt3); +static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); +static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); +static Cal3Bundler cal3(1.0, 2.0, 3.0); +static Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); +static Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); +static CalibratedCamera cal5(Pose3(rt3, pt3)); -Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); -Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); -Cal3Bundler cal3(1.0, 2.0, 3.0); -Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); -Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); -CalibratedCamera cal5(Pose3(rt3, pt3)); - -PinholeCamera cam1(pose3, cal1); -StereoCamera cam2(pose3, cal4ptr); -StereoPoint2 spt(1.0, 2.0, 3.0); +static PinholeCamera cam1(pose3, cal1); +static StereoCamera cam2(pose3, cal4ptr); +static StereoPoint2 spt(1.0, 2.0, 3.0); /* ************************************************************************* */ TEST (Serialization, text_geometry) { diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 6cc00de8a..a1647fa47 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -133,6 +133,26 @@ TEST( SimpleCamera, Dproject_point_pose) CHECK(assert_equal(Dpoint, numerical_point,1e-7)); } +/* ************************************************************************* */ +TEST( SimpleCamera, simpleCamera) +{ + Cal3_S2 K(468.2,427.2,91.2,300,200); + Rot3 R( + 0.41380,0.90915,0.04708, + -0.57338,0.22011,0.78917, + 0.70711,-0.35355,0.61237); + Point3 T(1000,2000,1500); + SimpleCamera expected(Pose3(R.inverse(),T),K); + // H&Z example, 2nd edition, page 163 + Matrix P = Matrix_(3,4, + 3.53553e2, 3.39645e2, 2.77744e2, -1.44946e6, + -1.03528e2, 2.33212e1, 4.59607e2, -6.32525e5, + 7.07107e-1, -3.53553e-1,6.12372e-1, -9.18559e2); + SimpleCamera actual = simpleCamera(P); + // Note precision of numbers given in book + CHECK(assert_equal(expected, actual,1e-1)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 2446ff122..29a8344d4 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -74,21 +74,21 @@ TEST( StereoCamera, project) /* ************************************************************************* */ -Pose3 camera1(Matrix_(3,3, +static Pose3 camera1(Matrix_(3,3, 1., 0., 0., 0.,-1., 0., 0., 0.,-1. ), Point3(0,0,6.25)); -Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); -StereoCamera stereoCam(Pose3(), K); +static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); +static StereoCamera stereoCam(Pose3(), K); // point X Y Z in meters -Point3 p(0, 0, 5); +static Point3 p(0, 0, 5); /* ************************************************************************* */ -StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); } +static StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); } TEST( StereoCamera, Dproject_stereo_pose) { Matrix expected = numericalDerivative21(project_,stereoCam, p); diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 0aa322e87..3a5b9b0da 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -26,7 +26,7 @@ namespace gtsam { -/** + /** * A permutation reorders variables, for example to reduce fill-in during * elimination. To save computation, the permutation can be applied to * the necessary data structures only once, then multiple computations @@ -151,8 +151,8 @@ public: */ Permutation::shared_ptr inverse() const; - const_iterator begin() const { return rangeIndices_.begin(); } /// permuted(permutation, container); - * permuted[index1]; - * permuted[index2]; - * which is equivalent to: - * container[permutation[index1]]; - * container[permutation[index2]]; - * but more concise. - */ -template -class Permuted { - Permutation permutation_; - CONTAINER& container_; -public: - typedef typename CONTAINER::iterator::value_type value_type; - - /** Construct as a permuted view on the Container. The permutation is copied - * but only a reference to the container is stored. - */ - Permuted(const Permutation& permutation, CONTAINER& container) : permutation_(permutation), container_(container) {} - - /** Construct as a view on the Container with an identity permutation. Only - * a reference to the container is stored. - */ - Permuted(CONTAINER& container) : permutation_(Permutation::Identity(container.size())), container_(container) {} - - /** Print */ - void print(const std::string& str = "") const { - std::cout << str; - permutation_.print(" permutation: "); - container_.print(" container: "); - } - - /** Access the container through the permutation */ - value_type& operator[](size_t index) { return container_[permutation_[index]]; } - - /** Access the container through the permutation (const version) */ - const value_type& operator[](size_t index) const { return container_[permutation_[index]]; } - - /** Assignment operator for cloning in ISAM2 */ - Permuted operator=(const Permuted& other) { - permutation_ = other.permutation_; - container_ = other.container_; - return *this; - } - - /** Permute this view by applying a permutation to the underlying permutation */ - void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); } - - /** Access the underlying container */ - CONTAINER* operator->() { return &container_; } - - /** Access the underlying container (const version) */ - const CONTAINER* operator->() const { return &container_; } - - /** Size of the underlying container */ - size_t size() const { return container_.size(); } - - /** Access to the underlying container */ - CONTAINER& container() { return container_; } - - /** Access to the underlying container (const version) */ - const CONTAINER& container() const { return container_; } - - /** Access the underlying permutation */ - Permutation& permutation() { return permutation_; } - const Permutation& permutation() const { return permutation_; } -}; - - } diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp index b58a69b6d..00f3439a0 100644 --- a/gtsam/inference/SymbolicFactorGraph.cpp +++ b/gtsam/inference/SymbolicFactorGraph.cpp @@ -85,8 +85,8 @@ namespace gtsam { "IndexFactor::CombineAndEliminate called on factors with no variables."); vector newKeys(keys.begin(), keys.end()); - return make_pair(new IndexConditional(newKeys, nrFrontals), - new IndexFactor(newKeys.begin() + nrFrontals, newKeys.end())); + return make_pair(boost::make_shared(newKeys, nrFrontals), + boost::make_shared(newKeys.begin() + nrFrontals, newKeys.end())); } /* ************************************************************************* */ diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index c7ef90e9f..a29d5e432 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -18,37 +18,12 @@ #include #include +#include namespace gtsam { using namespace std; -/* ************************************************************************* */ -VariableIndex::VariableIndex(const VariableIndex& other) : - index_(indexUnpermuted_) { - *this = other; -} - -/* ************************************************************************* */ -VariableIndex& VariableIndex::operator=(const VariableIndex& rhs) { - index_ = rhs.index_; - nFactors_ = rhs.nFactors_; - nEntries_ = rhs.nEntries_; - return *this; -} - -/* ************************************************************************* */ -void VariableIndex::permute(const Permutation& permutation) { -#ifndef NDEBUG - // Assert that the permutation does not leave behind any non-empty variables, - // otherwise the nFactors and nEntries counts would be incorrect. - for(Index j=0; jindex_.size(); ++j) - if(find(permutation.begin(), permutation.end(), j) == permutation.end()) - assert(this->operator[](j).empty()); -#endif - index_.permute(permutation); -} - /* ************************************************************************* */ bool VariableIndex::equals(const VariableIndex& other, double tol) const { if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) { @@ -66,17 +41,13 @@ bool VariableIndex::equals(const VariableIndex& other, double tol) const { /* ************************************************************************* */ void VariableIndex::print(const string& str) const { - cout << str << "\n"; + cout << str; cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; - Index var = 0; - BOOST_FOREACH(const Factors& variable, index_.container()) { - Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var); - assert(rvar != index_.permutation().end()); - cout << "var " << (rvar-index_.permutation().begin()) << ":"; - BOOST_FOREACH(const size_t factor, variable) + for(Index var = 0; var < size(); ++var) { + cout << "var " << var << ":"; + BOOST_FOREACH(const size_t factor, index_[var]) cout << " " << factor; cout << "\n"; - ++ var; } cout << flush; } @@ -85,7 +56,7 @@ void VariableIndex::print(const string& str) const { void VariableIndex::outputMetisFormat(ostream& os) const { os << size() << " " << nFactors() << "\n"; // run over variables, which will be hyper-edges. - BOOST_FOREACH(const Factors& variable, index_.container()) { + BOOST_FOREACH(const Factors& variable, index_) { // every variable is a hyper-edge covering its factors BOOST_FOREACH(const size_t factor, variable) os << (factor+1) << " "; // base 1 @@ -94,4 +65,15 @@ void VariableIndex::outputMetisFormat(ostream& os) const { os << flush; } +/* ************************************************************************* */ +void VariableIndex::permuteInPlace(const Permutation& permutation) { + // Create new index and move references to data into it in permuted order + vector newIndex(this->size()); + for(Index i = 0; i < newIndex.size(); ++i) + newIndex[i].swap(this->index_[permutation[i]]); + + // Move reference to entire index into the VariableIndex + index_.swap(newIndex); +} + } diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index c9efc6b22..be47cbd5a 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -22,10 +22,12 @@ #include #include -#include +#include namespace gtsam { + class Permutation; + /** * The VariableIndex class computes and stores the block column structure of a * factor graph. The factor graph stores a collection of factors, each of @@ -44,8 +46,7 @@ public: typedef Factors::const_iterator Factor_const_iterator; protected: - std::vector indexUnpermuted_; - Permuted > index_; // Permuted view of indexUnpermuted. + std::vector index_; size_t nFactors_; // Number of factors in the original factor graph. size_t nEntries_; // Sum of involved variable counts of each factor. @@ -55,7 +56,7 @@ public: /// @{ /** Default constructor, creates an empty VariableIndex */ - VariableIndex() : index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {} + VariableIndex() : nFactors_(0), nEntries_(0) {} /** * Create a VariableIndex that computes and stores the block column structure @@ -70,16 +71,6 @@ public: */ template VariableIndex(const FactorGraph& factorGraph); - /** - * Copy constructor - */ - VariableIndex(const VariableIndex& other); - - /** - * Assignment operator - */ - VariableIndex& operator=(const VariableIndex& rhs); - /// @} /// @name Standard Interface /// @{ @@ -120,9 +111,6 @@ public: /// @name Advanced Interface /// @{ - /** Access a list of factors by variable */ - Factors& operator[](Index variable) { checkVar(variable); return index_[variable]; } - /** * Augment the variable index with new factors. This can be used when * solving problems incrementally. @@ -137,11 +125,8 @@ public: */ template void remove(const CONTAINER& indices, const FactorGraph& factors); - /** - * Apply a variable permutation. Does not rearrange data, just permutes - * future lookups by variable. - */ - void permute(const Permutation& permutation); + /// Permute the variables in the VariableIndex according to the given permutation + void permuteInPlace(const Permutation& permutation); protected: Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } /// void fill(const FactorGraph& factorGraph); /// @} @@ -183,7 +168,7 @@ void VariableIndex::fill(const FactorGraph& factorGraph) { /* ************************************************************************* */ template VariableIndex::VariableIndex(const FactorGraph& factorGraph) : - index_(indexUnpermuted_), nFactors_(0), nEntries_(0) { + nFactors_(0), nEntries_(0) { // If the factor graph is empty, return an empty index because inside this // if block we assume at least one factor. @@ -200,8 +185,7 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) : } // Allocate array - index_.container().resize(maxVar+1); - index_.permutation() = Permutation::Identity(maxVar+1); + index_.resize(maxVar+1); fill(factorGraph); } @@ -210,7 +194,7 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph) : /* ************************************************************************* */ template VariableIndex::VariableIndex(const FactorGraph& factorGraph, Index nVariables) : - indexUnpermuted_(nVariables), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) { + index_(nVariables), nFactors_(0), nEntries_(0) { fill(factorGraph); } @@ -232,11 +216,7 @@ void VariableIndex::augment(const FactorGraph& factors) { } // Allocate index - Index originalSize = index_.size(); - index_.container().resize(std::max(index_.size(), maxVar+1)); - index_.permutation().resize(index_.container().size()); - for(Index var=originalSize; var SymbolicISAM; /* ************************************************************************* */ // Some numbers that should be consistent among all smoother tests -double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 = +static double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 = 0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1; /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 40d40a3a7..e167d8ad8 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -182,34 +182,18 @@ JacobianFactor::shared_ptr GaussianConditional::toFactor() const { return JacobianFactor::shared_ptr(new JacobianFactor(*this)); } -/* ************************************************************************* */ -template -inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES& x) { - - // Helper function to solve-in-place on a VectorValues or Permuted, - // called by GaussianConditional::solveInPlace(VectorValues&) and by - // GaussianConditional::solveInPlace(Permuted&). - - static const bool debug = false; - if(debug) conditional.print("Solving conditional in place"); - Vector xS = internal::extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents()); - xS = conditional.get_d() - conditional.get_S() * xS; - Vector soln = conditional.get_R().triangularView().solve(xS); - if(debug) { - gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on "); - gtsam::print(soln, "full back-substitution solution: "); - } - internal::writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals()); -} - /* ************************************************************************* */ void GaussianConditional::solveInPlace(VectorValues& x) const { - doSolveInPlace(*this, x); // Call helper version above -} - -/* ************************************************************************* */ -void GaussianConditional::solveInPlace(Permuted& x) const { - doSolveInPlace(*this, x); // Call helper version above + static const bool debug = false; + if(debug) this->print("Solving conditional in place"); + Vector xS = internal::extractVectorValuesSlices(x, this->beginParents(), this->endParents()); + xS = this->get_d() - this->get_S() * xS; + Vector soln = this->get_R().triangularView().solve(xS); + if(debug) { + gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on "); + gtsam::print(soln, "full back-substitution solution: "); + } + internal::writeVectorValuesSlices(soln, x, this->beginFrontals(), this->endFrontals()); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 5b7801156..cde4f266f 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -196,23 +196,6 @@ public: */ void solveInPlace(VectorValues& x) const; - /** - * Solves a conditional Gaussian and writes the solution into the entries of - * \c x for each frontal variable of the conditional (version for permuted - * VectorValues). The parents are assumed to have already been solved in - * and their values are read from \c x. This function works for multiple - * frontal variables. - * - * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2, - * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator - * variables of this conditional, this solve function computes - * \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution. - * - * @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the - * solution \f$ x_f \f$ will be written. - */ - void solveInPlace(Permuted& x) const; - // functions for transpose backsubstitution /** diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 660eb25f7..c5f726252 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -84,7 +84,7 @@ namespace gtsam { protected: - SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix + noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix std::vector firstNonzeroBlocks_; AbMatrix matrix_; // the full matrix corresponding to the factor BlockAb Ab_; // the block view of the full matrix diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 9d39b5674..01799148a 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -19,6 +19,8 @@ #pragma once #include +#include +#include #include #include diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 1413d1517..15925c644 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -17,10 +17,12 @@ */ #include +#include #include using namespace std; -using namespace gtsam; + +namespace gtsam { /* ************************************************************************* */ VectorValues::VectorValues(const VectorValues& other) { @@ -166,20 +168,24 @@ void VectorValues::operator+=(const VectorValues& c) { } /* ************************************************************************* */ -VectorValues& VectorValues::operator=(const Permuted& rhs) { - if(this->size() != rhs.size()) - throw std::invalid_argument("VectorValues assignment from Permuted requires pre-allocation, see documentation."); - for(size_t j=0; jsize(); ++j) { - if(exists(j)) { - SubVector& l(this->at(j)); - const SubVector& r(rhs[j]); - if(l.rows() != r.rows()) - throw std::invalid_argument("VectorValues assignment from Permuted requires pre-allocation, see documentation."); - l = r; - } else { - if(rhs.container().exists(rhs.permutation()[j])) - throw std::invalid_argument("VectorValues assignment from Permuted requires pre-allocation, see documentation."); - } - } - return *this; +VectorValues VectorValues::permute(const Permutation& permutation) const { + // Create result and allocate space + VectorValues lhs; + lhs.values_.resize(this->dim()); + lhs.maps_.reserve(this->size()); + + // Copy values from this VectorValues to the permuted VectorValues + size_t lhsPos = 0; + for(size_t i = 0; i < this->size(); ++i) { + // Map the next LHS subvector to the next slice of the LHS vector + lhs.maps_.push_back(SubVector(lhs.values_, lhsPos, this->at(permutation[i]).size())); + // Copy the data from the RHS subvector to the LHS subvector + lhs.maps_[i] = this->at(permutation[i]); + // Increment lhs position + lhsPos += lhs.maps_[i].size(); + } + + return lhs; } + +} \ No newline at end of file diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 04c280577..463080db9 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -19,7 +19,6 @@ #include #include -#include #include #include @@ -29,6 +28,9 @@ namespace gtsam { + // Forward declarations + class Permutation; + /** * This class represents a collection of vector-valued variables associated * each with a unique integer index. It is typically used to store the variables @@ -288,10 +290,11 @@ namespace gtsam { */ void operator+=(const VectorValues& c); - /** Assignment operator from Permuted, requires the dimensions - * of the assignee to already be properly pre-allocated. - */ - VectorValues& operator=(const Permuted& rhs); + /** + * Permute the entries of this VectorValues, returns a new VectorValues as + * the result. + */ + VectorValues permute(const Permutation& permutation) const; /// @} diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 93030b598..8b0c5e239 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -267,55 +267,6 @@ TEST( GaussianConditional, solve_multifrontal ) } -/* ************************************************************************* */ -TEST( GaussianConditional, solve_multifrontal_permuted ) -{ - // create full system, 3 variables, 2 frontals, all 2 dim - Matrix full_matrix = Matrix_(4, 7, - 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, - 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, - 0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3, - 0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4); - - // 3 variables, all dim=2 - vector dims; dims += 2, 2, 2, 1; - GaussianConditional::rsd_type matrices(full_matrix, dims.begin(), dims.end()); - Vector sigmas = ones(4); - vector cgdims; cgdims += _x_, _x1_, _l1_; - GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas); - - EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d())); - - // partial solution - Vector sl1 = Vector_(2, 9.0, 10.0); - - // elimination order; _x_, _x1_, _l1_ - VectorValues actualUnpermuted(vector(3, 2)); - Permutation permutation(3); - permutation[0] = 2; - permutation[1] = 0; - permutation[2] = 1; - Permuted actual(permutation, actualUnpermuted); - actual[_x_] = Vector_(2, 0.1, 0.2); // rhs - actual[_x1_] = Vector_(2, 0.3, 0.4); // rhs - actual[_l1_] = sl1; // parent - - VectorValues expectedUnpermuted(vector(3, 2)); - Permuted expected(permutation, expectedUnpermuted); - expected[_x_] = Vector_(2, -3.1,-3.4); - expected[_x1_] = Vector_(2, -11.9,-13.2); - expected[_l1_] = sl1; - - // verify indices/size - EXPECT_LONGS_EQUAL(3, cg.size()); - EXPECT_LONGS_EQUAL(4, cg.dim()); - - // solve and verify - cg.solveInPlace(actual); - EXPECT(assert_equal(expected.container(), actual.container(), tol)); - -} - /* ************************************************************************* */ TEST( GaussianConditional, solveTranspose ) { static const Index _y_=1; diff --git a/gtsam/linear/tests/testGaussianJunctionTree.cpp b/gtsam/linear/tests/testGaussianJunctionTree.cpp index b49d7fff0..c4ddc360b 100644 --- a/gtsam/linear/tests/testGaussianJunctionTree.cpp +++ b/gtsam/linear/tests/testGaussianJunctionTree.cpp @@ -35,7 +35,7 @@ using namespace gtsam; static const Index x2=0, x1=1, x3=2, x4=3; -GaussianFactorGraph createChain() { +static GaussianFactorGraph createChain() { typedef GaussianFactorGraph::sharedFactor Factor; SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 0.5); diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index a646773f2..e8a53c504 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -47,11 +47,11 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* ************************************************************************* */ // example noise models -noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); -noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); -noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); -noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1)); -noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); +static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); +static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); +static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); +static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1)); +static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); /* ************************************************************************* */ TEST (Serialization, noiseModels) { diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index a92b81ddc..17230854a 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -421,52 +421,31 @@ TEST(VectorValues, hasSameStructure) { EXPECT(!v1.hasSameStructure(VectorValues())); } + /* ************************************************************************* */ -TEST(VectorValues, permuted_combined) { - Vector v1 = Vector_(3, 1.0,2.0,3.0); - Vector v2 = Vector_(2, 4.0,5.0); - Vector v3 = Vector_(4, 6.0,7.0,8.0,9.0); +TEST(VectorValues, permute) { - vector dims(3); dims[0]=3; dims[1]=2; dims[2]=4; - VectorValues combined(dims); - combined[0] = v1; - combined[1] = v2; - combined[2] = v3; + VectorValues original; + original.insert(0, Vector_(1, 1.0)); + original.insert(1, Vector_(2, 2.0, 3.0)); + original.insert(2, Vector_(2, 4.0, 5.0)); + original.insert(3, Vector_(2, 6.0, 7.0)); - Permutation perm1(3); - perm1[0] = 1; - perm1[1] = 2; - perm1[2] = 0; + VectorValues expected; + expected.insert(0, Vector_(2, 4.0, 5.0)); // from 2 + expected.insert(1, Vector_(1, 1.0)); // from 0 + expected.insert(2, Vector_(2, 6.0, 7.0)); // from 3 + expected.insert(3, Vector_(2, 2.0, 3.0)); // from 1 - Permutation perm2(3); - perm2[0] = 1; - perm2[1] = 2; - perm2[2] = 0; + Permutation permutation(4); + permutation[0] = 2; + permutation[1] = 0; + permutation[2] = 3; + permutation[3] = 1; - Permuted permuted1(combined); - CHECK(assert_equal(v1, permuted1[0])) - CHECK(assert_equal(v2, permuted1[1])) - CHECK(assert_equal(v3, permuted1[2])) + VectorValues actual = original.permute(permutation); - permuted1.permute(perm1); - CHECK(assert_equal(v1, permuted1[2])) - CHECK(assert_equal(v2, permuted1[0])) - CHECK(assert_equal(v3, permuted1[1])) - - permuted1.permute(perm2); - CHECK(assert_equal(v1, permuted1[1])) - CHECK(assert_equal(v2, permuted1[2])) - CHECK(assert_equal(v3, permuted1[0])) - - Permuted permuted2(perm1, combined); - CHECK(assert_equal(v1, permuted2[2])) - CHECK(assert_equal(v2, permuted2[0])) - CHECK(assert_equal(v3, permuted2[1])) - - permuted2.permute(perm2); - CHECK(assert_equal(v1, permuted2[1])) - CHECK(assert_equal(v2, permuted2[2])) - CHECK(assert_equal(v3, permuted2[0])) + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 8b3bd4a0d..3c373e0ba 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -29,8 +29,8 @@ namespace gtsam { /* ************************************************************************* */ void ISAM2::Impl::AddVariables( - const Values& newTheta, Values& theta, Permuted& delta, - Permuted& deltaNewton, Permuted& deltaGradSearch, vector& replacedKeys, + const Values& newTheta, Values& theta, VectorValues& delta, + VectorValues& deltaNewton, VectorValues& deltaGradSearch, vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); @@ -40,28 +40,21 @@ void ISAM2::Impl::AddVariables( std::vector dims(newTheta.dims(*newTheta.orderingArbitrary())); if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl; const size_t newDim = accumulate(dims.begin(), dims.end(), 0); - const size_t originalDim = delta->dim(); - const size_t originalnVars = delta->size(); - delta.container().append(dims); - delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); - delta.permutation().resize(originalnVars + newTheta.size()); - deltaNewton.container().append(dims); - deltaNewton.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); - deltaNewton.permutation().resize(originalnVars + newTheta.size()); - deltaGradSearch.container().append(dims); - deltaGradSearch.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); - deltaGradSearch.permutation().resize(originalnVars + newTheta.size()); + const size_t originalDim = delta.dim(); + const size_t originalnVars = delta.size(); + delta.append(dims); + delta.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); + deltaNewton.append(dims); + deltaNewton.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); + deltaGradSearch.append(dims); + deltaGradSearch.vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); { Index nextVar = originalnVars; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { - delta.permutation()[nextVar] = nextVar; - deltaNewton.permutation()[nextVar] = nextVar; - deltaGradSearch.permutation()[nextVar] = nextVar; ordering.insert(key_value.key, nextVar); if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl; ++ nextVar; } - assert(delta.permutation().size() == delta.container().size()); assert(ordering.nVars() == delta.size()); assert(ordering.size() == delta.size()); } @@ -82,7 +75,7 @@ FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const N } /* ************************************************************************* */ -FastSet ISAM2::Impl::CheckRelinearizationFull(const Permuted& delta, const Ordering& ordering, +FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { FastSet relinKeys; @@ -110,7 +103,7 @@ FastSet ISAM2::Impl::CheckRelinearizationFull(const Permuted& relinKeys, double threshold, const Permuted& delta, const ISAM2Clique::shared_ptr& clique) { +void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) { // Check the current clique for relinearization bool relinearize = false; @@ -131,7 +124,7 @@ void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double thres } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const Permuted& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) { +void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const VectorValues& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) { // Check the current clique for relinearization bool relinearize = false; @@ -163,7 +156,7 @@ void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted& delta, const Ordering& ordering, +FastSet ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { FastSet relinKeys; @@ -201,13 +194,13 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, } /* ************************************************************************* */ -void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted& delta, const Ordering& ordering, - const vector& mask, boost::optional&> invalidateIfDebug, const KeyFormatter& keyFormatter) { +void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const Ordering& ordering, + const vector& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions // if we try to re-use them. #ifdef NDEBUG - invalidateIfDebug = boost::optional&>(); + invalidateIfDebug = boost::none; #endif assert(values.size() == ordering.nVars()); @@ -304,7 +297,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, toc(4,"ccolamd permutations"); tic(5,"permute affected variable index"); - affectedFactorsIndex.permute(*affectedColamd); + affectedFactorsIndex.permuteInPlace(*affectedColamd); toc(5,"permute affected variable index"); tic(6,"permute affected factors"); @@ -354,25 +347,13 @@ inline static void optimizeInPlace(const boost::shared_ptr& clique, } /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold) { +size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, VectorValues& delta, double wildfireThreshold) { size_t lastBacksubVariableCount; if (wildfireThreshold <= 0.0) { // Threshold is zero or less, so do a full recalculation - // Collect dimensions and allocate new VectorValues - vector dims(delta.size()); - for(size_t j=0; jdim(j); - VectorValues newDelta(dims); - - // Optimize full solution delta - internal::optimizeInPlace(root, newDelta); - - // Copy solution into delta - delta.permutation() = Permutation::Identity(delta.size()); - delta.container() = newDelta; - + internal::optimizeInPlace(root, delta); lastBacksubVariableCount = delta.size(); } else { @@ -380,8 +361,8 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std: lastBacksubVariableCount = optimizeWildfire(root, wildfireThreshold, replacedKeys, delta); // modifies delta_ #ifndef NDEBUG - for(size_t j=0; j)).all()); + for(size_t j=0; j)).all()); #endif } @@ -394,7 +375,7 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std: /* ************************************************************************* */ namespace internal { void updateDoglegDeltas(const boost::shared_ptr& clique, std::vector& replacedKeys, - const VectorValues& grad, Permuted& deltaNewton, Permuted& RgProd, size_t& varsUpdated) { + const VectorValues& grad, VectorValues& deltaNewton, VectorValues& RgProd, size_t& varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need // update deltas and recurse to children, but if not, we do not need to @@ -433,7 +414,7 @@ void updateDoglegDeltas(const boost::shared_ptr& clique, std::vecto /* ************************************************************************* */ size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, - Permuted& deltaNewton, Permuted& RgProd) { + VectorValues& deltaNewton, VectorValues& RgProd) { // Get gradient VectorValues grad = *allocateVectorValues(isam); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 0aafb3f35..fdb39d855 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -46,10 +46,10 @@ struct ISAM2::Impl { * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, - Permuted& deltaNewton, Permuted& deltaGradSearch, std::vector& replacedKeys, + static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta, + VectorValues& deltaNewton, VectorValues& deltaGradSearch, std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); - + /** * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol * in each NonlinearFactor, obtains the index by calling ordering[symbol]. @@ -68,7 +68,7 @@ struct ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationFull(const Permuted& delta, const Ordering& ordering, + static FastSet CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** @@ -82,7 +82,7 @@ struct ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted& delta, const Ordering& ordering, + static FastSet CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** @@ -115,9 +115,9 @@ struct ISAM2::Impl { * recalculate its delta. * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void ExpmapMasked(Values& values, const Permuted& delta, + static void ExpmapMasked(Values& values, const VectorValues& delta, const Ordering& ordering, const std::vector& mask, - boost::optional&> invalidateIfDebug = boost::optional&>(), + boost::optional invalidateIfDebug = boost::none, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** @@ -137,10 +137,10 @@ struct ISAM2::Impl { static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode, bool useQR); - static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); + static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, VectorValues& delta, double wildfireThreshold); static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, - Permuted& deltaNewton, Permuted& RgProd); + VectorValues& deltaNewton, VectorValues& RgProd); }; diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 3520ffb84..ba0071b51 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -37,7 +37,7 @@ VALUE ISAM2::calculateEstimate(Key key) const { namespace internal { template void optimizeWildfire(const boost::shared_ptr& clique, double threshold, - std::vector& changed, const std::vector& replaced, Permuted& delta, int& count) { + std::vector& changed, const std::vector& replaced, VectorValues& delta, int& count) { // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the // cliques in the children need to be processed @@ -114,7 +114,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, /* ************************************************************************* */ template -int optimizeWildfire(const boost::shared_ptr& root, double threshold, const std::vector& keys, Permuted& delta) { +int optimizeWildfire(const boost::shared_ptr& root, double threshold, const std::vector& keys, VectorValues& delta) { std::vector changed(keys.size(), false); int count = 0; // starting from the root, call optimize on each conditional diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 07e0f518d..0c3c198aa 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -41,7 +41,6 @@ static const double batchThreshold = 0.65; /* ************************************************************************* */ ISAM2::ISAM2(const ISAM2Params& params): - delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_), deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) { if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; @@ -49,15 +48,13 @@ ISAM2::ISAM2(const ISAM2Params& params): /* ************************************************************************* */ ISAM2::ISAM2(): - delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_), deltaDoglegUptodate_(true), deltaUptodate_(true) { if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; } /* ************************************************************************* */ -ISAM2::ISAM2(const ISAM2& other): - delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_) { +ISAM2::ISAM2(const ISAM2& other) { *this = other; } @@ -308,12 +305,12 @@ boost::shared_ptr > ISAM2::recalculate( // Reorder tic(2,"permute global variable index"); - variableIndex_.permute(*colamd); + variableIndex_.permuteInPlace(*colamd); toc(2,"permute global variable index"); tic(3,"permute delta"); - delta_.permute(*colamd); - deltaNewton_.permute(*colamd); - RgProd_.permute(*colamd); + delta_ = delta_.permute(*colamd); + deltaNewton_ = deltaNewton_.permute(*colamd); + RgProd_ = RgProd_.permute(*colamd); toc(3,"permute delta"); tic(4,"permute ordering"); ordering_.permuteWithInverse(*colamdInverse); @@ -429,12 +426,12 @@ boost::shared_ptr > ISAM2::recalculate( // re-eliminate. The reordered variables are also mentioned in the // orphans and the leftover cached factors. tic(3,"permute global variable index"); - variableIndex_.permute(partialSolveResult.fullReordering); + variableIndex_.permuteInPlace(partialSolveResult.fullReordering); toc(3,"permute global variable index"); tic(4,"permute delta"); - delta_.permute(partialSolveResult.fullReordering); - deltaNewton_.permute(partialSolveResult.fullReordering); - RgProd_.permute(partialSolveResult.fullReordering); + delta_ = delta_.permute(partialSolveResult.fullReordering); + deltaNewton_ = deltaNewton_.permute(partialSolveResult.fullReordering); + RgProd_ = RgProd_.permute(partialSolveResult.fullReordering); toc(4,"permute delta"); tic(5,"permute ordering"); ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); @@ -723,8 +720,7 @@ void ISAM2::updateDelta(bool forceFullSolve) const { tic(2, "Copy dx_d"); // Update Delta and linear step doglegDelta_ = doglegResult.Delta; - delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation - delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution + delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution toc(2, "Copy dx_d"); } @@ -739,7 +735,7 @@ Values ISAM2::calculateEstimate() const { Values ret(theta_); toc(1, "Copy Values"); tic(2, "getDelta"); - const Permuted& delta(getDelta()); + const VectorValues& delta(getDelta()); toc(2, "getDelta"); tic(3, "Expmap"); vector mask(ordering_.nVars(), true); @@ -756,7 +752,7 @@ Values ISAM2::calculateBestEstimate() const { } /* ************************************************************************* */ -const Permuted& ISAM2::getDelta() const { +const VectorValues& ISAM2::getDelta() const { if(!deltaUptodate_) updateDelta(); return delta_; @@ -829,7 +825,7 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) { tic(3, "Compute minimizing step size"); // Compute minimizing step size - double RgNormSq = isam.RgProd_.container().vector().squaredNorm(); + double RgNormSq = isam.RgProd_.vector().squaredNorm(); double step = -gradientSqNorm / RgNormSq; toc(3, "Compute minimizing step size"); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 5804640df..2fd3e0b23 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -206,7 +206,7 @@ struct ISAM2Result { * factors passed as \c newFactors to ISAM2::update(). These indices may be * used later to refer to the factors in order to remove them. */ - FastVector newFactorsIndices; + std::vector newFactorsIndices; /** A struct holding detailed results, which must be enabled with * ISAM2Params::enableDetailedResults. @@ -347,26 +347,16 @@ protected: /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */ VariableIndex variableIndex_; - /** The linear delta from the last linear solution, an update to the estimate in theta */ - VectorValues deltaUnpermuted_; + /** The linear delta from the last linear solution, an update to the estimate in theta + * + * This is \c mutable because it is a "cached" variable - it is not updated + * until either requested with getDelta() or calculateEstimate(), or needed + * during update() to evaluate whether to relinearize variables. + */ + mutable VectorValues delta_; - /** The permutation through which the deltaUnpermuted_ is - * referenced. - * - * Permuting Vector entries would be slow, so for performance we - * instead maintain this permutation through which we access the linear delta - * indirectly - * - * This is \c mutable because it is a "cached" variable - it is not updated - * until either requested with getDelta() or calculateEstimate(), or needed - * during update() to evaluate whether to relinearize variables. - */ - mutable Permuted delta_; - - VectorValues deltaNewtonUnpermuted_; - mutable Permuted deltaNewton_; - VectorValues RgProdUnpermuted_; - mutable Permuted RgProd_; + mutable VectorValues deltaNewton_; + mutable VectorValues RgProd_; mutable bool deltaDoglegUptodate_; /** Indicates whether the current delta is up-to-date, only used @@ -497,7 +487,7 @@ public: Values calculateBestEstimate() const; /** Access the current delta, computed during the last call to update */ - const Permuted& getDelta() const; + const VectorValues& getDelta() const; /** Access the set of nonlinear factors */ const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } @@ -555,7 +545,7 @@ void optimizeInPlace(const ISAM2& isam, VectorValues& delta); /// @return The number of variables that were solved for template int optimizeWildfire(const boost::shared_ptr& root, - double threshold, const std::vector& replaced, Permuted& delta); + double threshold, const std::vector& replaced, VectorValues& delta); /** * Optimize along the gradient direction, with a closed-form computation to diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index bb66943eb..58099d149 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -120,7 +120,7 @@ namespace gtsam { /* ************************************************************************* */ void Values::insert(Key j, const Value& val) { Key key = j; // Non-const duplicate to deal with non-const insert argument - std::pair insertResult = values_.insert(key, val.clone_()); + std::pair insertResult = values_.insert(key, val.clone_()); if(!insertResult.second) throw ValuesKeyAlreadyExists(j); } diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index bc023f115..57e2c0a34 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -47,13 +47,13 @@ typedef PinholeCamera PinholeCal3DS2; typedef PinholeCamera PinholeCal3Bundler; /* ************************************************************************* */ -Point3 pt3(1.0, 2.0, 3.0); -Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); -Pose3 pose3(rt3, pt3); +static Point3 pt3(1.0, 2.0, 3.0); +static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +static Pose3 pose3(rt3, pt3); -Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); -Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); -Cal3Bundler cal3(1.0, 2.0, 3.0); +static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); +static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); +static Cal3Bundler cal3(1.0, 2.0, 3.0); TEST (Serialization, TemplatedValues) { Values values; diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 986538448..b38dae990 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -28,18 +28,18 @@ using namespace std; using namespace gtsam; -Pose3 camera1(Matrix_(3,3, +static Pose3 camera1(Matrix_(3,3, 1., 0., 0., 0.,-1., 0., 0., 0.,-1. ), Point3(0,0,6.25)); -Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); -StereoCamera stereoCam(Pose3(), K); +static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); +static StereoCamera stereoCam(Pose3(), K); // point X Y Z in meters -Point3 p(0, 0, 5); +static Point3 p(0, 0, 5); static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); // Convenience for named keys diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index 41905f874..ac412fa7f 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -20,10 +20,11 @@ namespace gtsam { } /* ************************************************************************* */ - void AllDiff::print(const std::string& s) const { + void AllDiff::print(const std::string& s, + const IndexFormatter& formatter) const { std::cout << s << "AllDiff on "; BOOST_FOREACH (Index dkey, keys_) - std::cout << dkey << " "; + std::cout << formatter(dkey) << " "; std::cout << std::endl; } diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index fb5a47a59..1a560ace2 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -34,7 +34,8 @@ namespace gtsam { AllDiff(const DiscreteKeys& dkeys); // print - virtual void print(const std::string& s = "") const; + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = DefaultIndexFormatter) const; /// Calculate value = expensive ! virtual double operator()(const Values& values) const; diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index a2e260bcd..9ed2f79f1 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -33,9 +33,10 @@ namespace gtsam { } // print - virtual void print(const std::string& s = "") const { - std::cout << s << "BinaryAllDiff on " << keys_[0] << " and " << keys_[1] - << std::endl; + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = DefaultIndexFormatter) const { + std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " + << formatter(keys_[1]) << std::endl; } /// Calculate value diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index e43065f3b..dbc05e3f6 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -15,9 +15,10 @@ namespace gtsam { using namespace std; /* ************************************************************************* */ - void Domain::print(const string& s) const { -// cout << s << ": Domain on " << keys_[0] << " (j=" << keys_[0] -// << ") with values"; + void Domain::print(const string& s, + const IndexFormatter& formatter) const { +// cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" << +// formatter(keys_[0]) << ") with values"; // BOOST_FOREACH (size_t v,values_) cout << " " << v; // cout << endl; BOOST_FOREACH (size_t v,values_) cout << v; diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index a4f0c8054..85b35fe8c 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -66,7 +66,8 @@ namespace gtsam { } // print - virtual void print(const std::string& s = "") const; + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = DefaultIndexFormatter) const; bool contains(size_t value) const { return values_.count(value)>0; diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 6f6d5a3ff..81133e7f7 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -16,8 +16,9 @@ namespace gtsam { using namespace std; /* ************************************************************************* */ - void SingleValue::print(const string& s) const { - cout << s << "SingleValue on " << "j=" << keys_[0] + void SingleValue::print(const string& s, + const IndexFormatter& formatter) const { + cout << s << "SingleValue on " << "j=" << formatter(keys_[0]) << " with value " << value_ << endl; } diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index b229d8b79..1f6e362aa 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -42,7 +42,8 @@ namespace gtsam { } // print - virtual void print(const std::string& s = "") const; + virtual void print(const std::string& s = "", + const IndexFormatter& formatter = DefaultIndexFormatter) const; /// Calculate value virtual double operator()(const Values& values) const; diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp index bf9273ad6..6559754d5 100644 --- a/gtsam_unstable/discrete/tests/testScheduler.cpp +++ b/gtsam_unstable/discrete/tests/testScheduler.cpp @@ -149,7 +149,7 @@ TEST( schedulingExample, test) /* ************************************************************************* */ TEST( schedulingExample, smallFromFile) { - string path("../../../gtsam_unstable/discrete/examples/"); + string path(TOPSRCDIR "/gtsam_unstable/discrete/examples/"); Scheduler s(2, path + "small.csv"); // add areas diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 6ae4a7b20..7ec4e5317 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -41,10 +41,10 @@ using symbol_shorthand::L; /* ************************************************************************* */ // Some numbers that should be consistent among all smoother tests -double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 = +static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 = 0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1; -const double tol = 1e-4; +static const double tol = 1e-4; /* ************************************************************************* */ TEST_UNSAFE( ISAM, iSAM_smoother ) diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 2a5bf7962..994077777 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -139,112 +139,69 @@ TEST_UNSAFE(ISAM2, AddVariables) { // Create initial state Values theta; - theta.insert((0), Pose2(.1, .2, .3)); + theta.insert(0, Pose2(.1, .2, .3)); theta.insert(100, Point2(.4, .5)); Values newTheta; - newTheta.insert((1), Pose2(.6, .7, .8)); + newTheta.insert(1, Pose2(.6, .7, .8)); - VectorValues deltaUnpermuted; - deltaUnpermuted.insert(0, Vector_(3, .1, .2, .3)); - deltaUnpermuted.insert(1, Vector_(2, .4, .5)); + VectorValues delta; + delta.insert(0, Vector_(3, .1, .2, .3)); + delta.insert(1, Vector_(2, .4, .5)); - Permutation permutation(2); - permutation[0] = 1; - permutation[1] = 0; + VectorValues deltaNewton; + deltaNewton.insert(0, Vector_(3, .1, .2, .3)); + deltaNewton.insert(1, Vector_(2, .4, .5)); - Permuted delta(permutation, deltaUnpermuted); - - VectorValues deltaNewtonUnpermuted; - deltaNewtonUnpermuted.insert(0, Vector_(3, .1, .2, .3)); - deltaNewtonUnpermuted.insert(1, Vector_(2, .4, .5)); - - Permutation permutationNewton(2); - permutationNewton[0] = 1; - permutationNewton[1] = 0; - - Permuted deltaNewton(permutationNewton, deltaNewtonUnpermuted); - - VectorValues deltaRgUnpermuted; - deltaRgUnpermuted.insert(0, Vector_(3, .1, .2, .3)); - deltaRgUnpermuted.insert(1, Vector_(2, .4, .5)); - - Permutation permutationRg(2); - permutationRg[0] = 1; - permutationRg[1] = 0; - - Permuted deltaRg(permutationRg, deltaRgUnpermuted); + VectorValues deltaRg; + deltaRg.insert(0, Vector_(3, .1, .2, .3)); + deltaRg.insert(1, Vector_(2, .4, .5)); vector replacedKeys(2, false); - Ordering ordering; ordering += 100, (0); + Ordering ordering; ordering += 100, 0; ISAM2::Nodes nodes(2); // Verify initial state LONGS_EQUAL(0, ordering[100]); - LONGS_EQUAL(1, ordering[(0)]); - EXPECT(assert_equal(deltaUnpermuted[1], delta[ordering[100]])); - EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[(0)]])); + LONGS_EQUAL(1, ordering[0]); + EXPECT(assert_equal(delta[0], delta[ordering[100]])); + EXPECT(assert_equal(delta[1], delta[ordering[0]])); // Create expected state Values thetaExpected; - thetaExpected.insert((0), Pose2(.1, .2, .3)); + thetaExpected.insert(0, Pose2(.1, .2, .3)); thetaExpected.insert(100, Point2(.4, .5)); - thetaExpected.insert((1), Pose2(.6, .7, .8)); + thetaExpected.insert(1, Pose2(.6, .7, .8)); - VectorValues deltaUnpermutedExpected; - deltaUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaUnpermutedExpected.insert(1, Vector_(2, .4, .5)); - deltaUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); + VectorValues deltaExpected; + deltaExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaExpected.insert(1, Vector_(2, .4, .5)); + deltaExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - Permutation permutationExpected(3); - permutationExpected[0] = 1; - permutationExpected[1] = 0; - permutationExpected[2] = 2; + VectorValues deltaNewtonExpected; + deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaNewtonExpected.insert(1, Vector_(2, .4, .5)); + deltaNewtonExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - Permuted deltaExpected(permutationExpected, deltaUnpermutedExpected); - - VectorValues deltaNewtonUnpermutedExpected; - deltaNewtonUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaNewtonUnpermutedExpected.insert(1, Vector_(2, .4, .5)); - deltaNewtonUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - - Permutation permutationNewtonExpected(3); - permutationNewtonExpected[0] = 1; - permutationNewtonExpected[1] = 0; - permutationNewtonExpected[2] = 2; - - Permuted deltaNewtonExpected(permutationNewtonExpected, deltaNewtonUnpermutedExpected); - - VectorValues deltaRgUnpermutedExpected; - deltaRgUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaRgUnpermutedExpected.insert(1, Vector_(2, .4, .5)); - deltaRgUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - - Permutation permutationRgExpected(3); - permutationRgExpected[0] = 1; - permutationRgExpected[1] = 0; - permutationRgExpected[2] = 2; - - Permuted deltaRgExpected(permutationRgExpected, deltaRgUnpermutedExpected); + VectorValues deltaRgExpected; + deltaRgExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaRgExpected.insert(1, Vector_(2, .4, .5)); + deltaRgExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); vector replacedKeysExpected(3, false); - Ordering orderingExpected; orderingExpected += 100, (0), (1); + Ordering orderingExpected; orderingExpected += 100, 0, 1; - ISAM2::Nodes nodesExpected( - 3, ISAM2::sharedClique()); + ISAM2::Nodes nodesExpected(3, ISAM2::sharedClique()); // Expand initial state ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes); EXPECT(assert_equal(thetaExpected, theta)); - EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted)); - EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation())); - EXPECT(assert_equal(deltaNewtonUnpermutedExpected, deltaNewtonUnpermuted)); - EXPECT(assert_equal(deltaNewtonExpected.permutation(), deltaNewton.permutation())); - EXPECT(assert_equal(deltaRgUnpermutedExpected, deltaRgUnpermuted)); - EXPECT(assert_equal(deltaRgExpected.permutation(), deltaRg.permutation())); + EXPECT(assert_equal(deltaExpected, delta)); + EXPECT(assert_equal(deltaNewtonExpected, deltaNewton)); + EXPECT(assert_equal(deltaRgExpected, deltaRg)); EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); EXPECT(assert_equal(orderingExpected, ordering)); } diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 0f412a0b5..883011da2 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -52,7 +52,7 @@ using symbol_shorthand::L; C3 x1 : x2 C4 x7 : x6 */ -TEST( GaussianJunctionTree, constructor2 ) +TEST( GaussianJunctionTreeB, constructor2 ) { // create a graph Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); @@ -88,7 +88,7 @@ TEST( GaussianJunctionTree, constructor2 ) } /* ************************************************************************* */ -TEST( GaussianJunctionTree, optimizeMultiFrontal ) +TEST( GaussianJunctionTreeB, optimizeMultiFrontal ) { // create a graph GaussianFactorGraph fg; @@ -108,7 +108,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal ) } /* ************************************************************************* */ -TEST( GaussianJunctionTree, optimizeMultiFrontal2) +TEST( GaussianJunctionTreeB, optimizeMultiFrontal2) { // create a graph example::Graph nlfg = createNonlinearFactorGraph(); @@ -126,7 +126,7 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2) } /* ************************************************************************* */ -TEST(GaussianJunctionTree, slamlike) { +TEST(GaussianJunctionTreeB, slamlike) { Values init; planarSLAM::Graph newfactors; planarSLAM::Graph fullgraph; @@ -188,7 +188,7 @@ TEST(GaussianJunctionTree, slamlike) { } /* ************************************************************************* */ -TEST(GaussianJunctionTree, simpleMarginal) { +TEST(GaussianJunctionTreeB, simpleMarginal) { typedef BayesTree GaussianBayesTree; diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 4679dd740..1efc40cad 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -37,7 +37,7 @@ typedef PriorFactor PosePrior; typedef NonlinearEquality PoseNLE; typedef boost::shared_ptr shared_poseNLE; -Symbol key('x',1); +static Symbol key('x',1); /* ************************************************************************* */ TEST ( NonlinearEquality, linearization ) { @@ -241,8 +241,8 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { } /* ************************************************************************* */ -SharedDiagonal hard_model = noiseModel::Constrained::All(2); -SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); +static SharedDiagonal hard_model = noiseModel::Constrained::All(2); +static SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, unary_basics ) { @@ -504,10 +504,10 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { } // make a realistic calibration matrix -double fov = 60; // degrees -size_t w=640,h=480; -Cal3_S2 K(fov,w,h); -boost::shared_ptr shK(new Cal3_S2(K)); +static double fov = 60; // degrees +static size_t w=640,h=480; +static Cal3_S2 K(fov,w,h); +static boost::shared_ptr shK(new Cal3_S2(K)); // typedefs for visual SLAM example typedef visualSLAM::Graph VGraph; diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 7650925d1..d9094111e 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -174,10 +174,10 @@ BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoF BOOST_CLASS_EXPORT(gtsam::Pose3) BOOST_CLASS_EXPORT(gtsam::Point3) -Point3 pt3(1.0, 2.0, 3.0); -Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); -Pose3 pose3(rt3, pt3); -Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); +static Point3 pt3(1.0, 2.0, 3.0); +static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +static Pose3 pose3(rt3, pt3); +static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); /* ************************************************************************* */ TEST (Serialization, visual_system) { diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 16dfbb8e5..757b488ba 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -4,7 +4,7 @@ find_package(Boost 1.42 COMPONENTS system filesystem thread REQUIRED) # Build the executable itself file(GLOB wrap_srcs "*.cpp") -list(REMOVE_ITEM wrap_srcs wrap.cpp) +list(REMOVE_ITEM wrap_srcs ${CMAKE_CURRENT_SOURCE_DIR}/wrap.cpp) add_library(wrap_lib STATIC ${wrap_srcs}) add_executable(wrap wrap.cpp) target_link_libraries(wrap wrap_lib ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}) @@ -19,7 +19,6 @@ install(FILES matlab.h DESTINATION include/wrap) # Build tests if (GTSAM_BUILD_TESTS) - add_definitions(-DTOPSRCDIR="${CMAKE_SOURCE_DIR}") set(wrap_local_libs wrap_lib ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}) gtsam_add_subdir_tests("wrap" "${wrap_local_libs}" "${wrap_local_libs}" "") endif(GTSAM_BUILD_TESTS) From 7ba7aa6cfc6c31ef20482589420ce56850b269b4 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sat, 30 Jun 2012 20:04:16 +0000 Subject: [PATCH 18/21] Updated wrap unit tests for new wrap changes --- wrap/tests/expected/Makefile | 65 ++++++++++--------- wrap/tests/expected/new_Point2_.cpp | 1 - wrap/tests/expected/new_Point3_.cpp | 1 - wrap/tests/expected/new_Test_.cpp | 1 - wrap/tests/expected_namespaces/Makefile | 21 +++--- .../tests/expected_namespaces/new_ClassD_.cpp | 1 - .../expected_namespaces/new_ns1ClassA_.cpp | 1 - .../expected_namespaces/new_ns1ClassB_.cpp | 1 - .../expected_namespaces/new_ns2ClassA_.cpp | 1 - .../expected_namespaces/new_ns2ClassC_.cpp | 1 - .../expected_namespaces/new_ns2ns3ClassB_.cpp | 1 - wrap/tests/testWrap.cpp | 14 ++-- wrap/wrap.cpp | 3 +- 13 files changed, 55 insertions(+), 57 deletions(-) diff --git a/wrap/tests/expected/Makefile b/wrap/tests/expected/Makefile index 0797d0dd3..57a606790 100644 --- a/wrap/tests/expected/Makefile +++ b/wrap/tests/expected/Makefile @@ -2,82 +2,83 @@ MEX = mex MEXENDING = mexa64 +PATH_TO_WRAP = /not_really_a_real_path/borg/gtsam/wrap mex_flags = -O5 all: Point2 Point3 Test # Point2 -new_Point2_.$(MEXENDING): new_Point2_.cpp +new_Point2_.$(MEXENDING): new_Point2_.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) new_Point2_.cpp -output new_Point2_ -@Point2/x.$(MEXENDING): @Point2/x.cpp +@Point2/x.$(MEXENDING): @Point2/x.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Point2/x.cpp -output @Point2/x -@Point2/y.$(MEXENDING): @Point2/y.cpp +@Point2/y.$(MEXENDING): @Point2/y.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Point2/y.cpp -output @Point2/y -@Point2/dim.$(MEXENDING): @Point2/dim.cpp +@Point2/dim.$(MEXENDING): @Point2/dim.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Point2/dim.cpp -output @Point2/dim -@Point2/returnChar.$(MEXENDING): @Point2/returnChar.cpp +@Point2/returnChar.$(MEXENDING): @Point2/returnChar.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Point2/returnChar.cpp -output @Point2/returnChar -@Point2/argChar.$(MEXENDING): @Point2/argChar.cpp +@Point2/argChar.$(MEXENDING): @Point2/argChar.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Point2/argChar.cpp -output @Point2/argChar -@Point2/argUChar.$(MEXENDING): @Point2/argUChar.cpp +@Point2/argUChar.$(MEXENDING): @Point2/argUChar.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Point2/argUChar.cpp -output @Point2/argUChar -@Point2/vectorConfusion.$(MEXENDING): @Point2/vectorConfusion.cpp +@Point2/vectorConfusion.$(MEXENDING): @Point2/vectorConfusion.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Point2/vectorConfusion.cpp -output @Point2/vectorConfusion Point2: new_Point2_.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING) @Point2/returnChar.$(MEXENDING) @Point2/argChar.$(MEXENDING) @Point2/argUChar.$(MEXENDING) @Point2/vectorConfusion.$(MEXENDING) # Point3 -new_Point3_.$(MEXENDING): new_Point3_.cpp +new_Point3_.$(MEXENDING): new_Point3_.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) new_Point3_.cpp -output new_Point3_ -Point3_staticFunction.$(MEXENDING): Point3_staticFunction.cpp +Point3_staticFunction.$(MEXENDING): Point3_staticFunction.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) Point3_staticFunction.cpp -output Point3_staticFunction -Point3_StaticFunctionRet.$(MEXENDING): Point3_StaticFunctionRet.cpp +Point3_StaticFunctionRet.$(MEXENDING): Point3_StaticFunctionRet.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) Point3_StaticFunctionRet.cpp -output Point3_StaticFunctionRet -@Point3/norm.$(MEXENDING): @Point3/norm.cpp +@Point3/norm.$(MEXENDING): @Point3/norm.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Point3/norm.cpp -output @Point3/norm Point3: new_Point3_.$(MEXENDING) Point3_staticFunction.$(MEXENDING) Point3_StaticFunctionRet.$(MEXENDING) @Point3/norm.$(MEXENDING) # Test -new_Test_.$(MEXENDING): new_Test_.cpp +new_Test_.$(MEXENDING): new_Test_.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) new_Test_.cpp -output new_Test_ -@Test/return_pair.$(MEXENDING): @Test/return_pair.cpp +@Test/return_pair.$(MEXENDING): @Test/return_pair.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_pair.cpp -output @Test/return_pair -@Test/return_bool.$(MEXENDING): @Test/return_bool.cpp +@Test/return_bool.$(MEXENDING): @Test/return_bool.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_bool.cpp -output @Test/return_bool -@Test/return_size_t.$(MEXENDING): @Test/return_size_t.cpp +@Test/return_size_t.$(MEXENDING): @Test/return_size_t.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_size_t.cpp -output @Test/return_size_t -@Test/return_int.$(MEXENDING): @Test/return_int.cpp +@Test/return_int.$(MEXENDING): @Test/return_int.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_int.cpp -output @Test/return_int -@Test/return_double.$(MEXENDING): @Test/return_double.cpp +@Test/return_double.$(MEXENDING): @Test/return_double.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_double.cpp -output @Test/return_double -@Test/return_string.$(MEXENDING): @Test/return_string.cpp +@Test/return_string.$(MEXENDING): @Test/return_string.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_string.cpp -output @Test/return_string -@Test/return_vector1.$(MEXENDING): @Test/return_vector1.cpp +@Test/return_vector1.$(MEXENDING): @Test/return_vector1.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_vector1.cpp -output @Test/return_vector1 -@Test/return_matrix1.$(MEXENDING): @Test/return_matrix1.cpp +@Test/return_matrix1.$(MEXENDING): @Test/return_matrix1.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_matrix1.cpp -output @Test/return_matrix1 -@Test/return_vector2.$(MEXENDING): @Test/return_vector2.cpp +@Test/return_vector2.$(MEXENDING): @Test/return_vector2.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_vector2.cpp -output @Test/return_vector2 -@Test/return_matrix2.$(MEXENDING): @Test/return_matrix2.cpp +@Test/return_matrix2.$(MEXENDING): @Test/return_matrix2.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_matrix2.cpp -output @Test/return_matrix2 -@Test/arg_EigenConstRef.$(MEXENDING): @Test/arg_EigenConstRef.cpp +@Test/arg_EigenConstRef.$(MEXENDING): @Test/arg_EigenConstRef.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/arg_EigenConstRef.cpp -output @Test/arg_EigenConstRef -@Test/return_field.$(MEXENDING): @Test/return_field.cpp +@Test/return_field.$(MEXENDING): @Test/return_field.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_field.cpp -output @Test/return_field -@Test/return_TestPtr.$(MEXENDING): @Test/return_TestPtr.cpp +@Test/return_TestPtr.$(MEXENDING): @Test/return_TestPtr.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_TestPtr.cpp -output @Test/return_TestPtr -@Test/return_Test.$(MEXENDING): @Test/return_Test.cpp +@Test/return_Test.$(MEXENDING): @Test/return_Test.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_Test.cpp -output @Test/return_Test -@Test/return_Point2Ptr.$(MEXENDING): @Test/return_Point2Ptr.cpp +@Test/return_Point2Ptr.$(MEXENDING): @Test/return_Point2Ptr.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_Point2Ptr.cpp -output @Test/return_Point2Ptr -@Test/create_ptrs.$(MEXENDING): @Test/create_ptrs.cpp +@Test/create_ptrs.$(MEXENDING): @Test/create_ptrs.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/create_ptrs.cpp -output @Test/create_ptrs -@Test/create_MixedPtrs.$(MEXENDING): @Test/create_MixedPtrs.cpp +@Test/create_MixedPtrs.$(MEXENDING): @Test/create_MixedPtrs.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/create_MixedPtrs.cpp -output @Test/create_MixedPtrs -@Test/return_ptrs.$(MEXENDING): @Test/return_ptrs.cpp +@Test/return_ptrs.$(MEXENDING): @Test/return_ptrs.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/return_ptrs.cpp -output @Test/return_ptrs -@Test/print.$(MEXENDING): @Test/print.cpp +@Test/print.$(MEXENDING): @Test/print.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @Test/print.cpp -output @Test/print Test: new_Test_.$(MEXENDING) @Test/return_pair.$(MEXENDING) @Test/return_bool.$(MEXENDING) @Test/return_size_t.$(MEXENDING) @Test/return_int.$(MEXENDING) @Test/return_double.$(MEXENDING) @Test/return_string.$(MEXENDING) @Test/return_vector1.$(MEXENDING) @Test/return_matrix1.$(MEXENDING) @Test/return_vector2.$(MEXENDING) @Test/return_matrix2.$(MEXENDING) @Test/arg_EigenConstRef.$(MEXENDING) @Test/return_field.$(MEXENDING) @Test/return_TestPtr.$(MEXENDING) @Test/return_Test.$(MEXENDING) @Test/return_Point2Ptr.$(MEXENDING) @Test/create_ptrs.$(MEXENDING) @Test/create_MixedPtrs.$(MEXENDING) @Test/return_ptrs.$(MEXENDING) @Test/print.$(MEXENDING) diff --git a/wrap/tests/expected/new_Point2_.cpp b/wrap/tests/expected/new_Point2_.cpp index 7d3af0038..11b2821a3 100644 --- a/wrap/tests/expected/new_Point2_.cpp +++ b/wrap/tests/expected/new_Point2_.cpp @@ -41,7 +41,6 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) self = new Shared(new Point2(x,y)); } collector.insert(self); - std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast (mxGetPr(out[0])) = self; } diff --git a/wrap/tests/expected/new_Point3_.cpp b/wrap/tests/expected/new_Point3_.cpp index 368b3069e..01316da5a 100644 --- a/wrap/tests/expected/new_Point3_.cpp +++ b/wrap/tests/expected/new_Point3_.cpp @@ -40,7 +40,6 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) self = new Shared(new Point3(x,y,z)); } collector.insert(self); - std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast (mxGetPr(out[0])) = self; } diff --git a/wrap/tests/expected/new_Test_.cpp b/wrap/tests/expected/new_Test_.cpp index a6bcc9549..e14e6b71d 100644 --- a/wrap/tests/expected/new_Test_.cpp +++ b/wrap/tests/expected/new_Test_.cpp @@ -42,7 +42,6 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) self = new Shared(new Test(a,b)); } collector.insert(self); - std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast (mxGetPr(out[0])) = self; } diff --git a/wrap/tests/expected_namespaces/Makefile b/wrap/tests/expected_namespaces/Makefile index dcc3b1dbd..55866228b 100644 --- a/wrap/tests/expected_namespaces/Makefile +++ b/wrap/tests/expected_namespaces/Makefile @@ -2,50 +2,51 @@ MEX = mex MEXENDING = mexa64 +PATH_TO_WRAP = /not_really_a_real_path/borg/gtsam/wrap mex_flags = -O5 all: ns1ClassA ns1ClassB ns2ClassA ns2ns3ClassB ns2ClassC ClassD # ns1ClassA -new_ns1ClassA_.$(MEXENDING): new_ns1ClassA_.cpp +new_ns1ClassA_.$(MEXENDING): new_ns1ClassA_.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) new_ns1ClassA_.cpp -output new_ns1ClassA_ ns1ClassA: new_ns1ClassA_.$(MEXENDING) # ns1ClassB -new_ns1ClassB_.$(MEXENDING): new_ns1ClassB_.cpp +new_ns1ClassB_.$(MEXENDING): new_ns1ClassB_.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) new_ns1ClassB_.cpp -output new_ns1ClassB_ ns1ClassB: new_ns1ClassB_.$(MEXENDING) # ns2ClassA -new_ns2ClassA_.$(MEXENDING): new_ns2ClassA_.cpp +new_ns2ClassA_.$(MEXENDING): new_ns2ClassA_.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) new_ns2ClassA_.cpp -output new_ns2ClassA_ -ns2ClassA_afunction.$(MEXENDING): ns2ClassA_afunction.cpp +ns2ClassA_afunction.$(MEXENDING): ns2ClassA_afunction.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) ns2ClassA_afunction.cpp -output ns2ClassA_afunction -@ns2ClassA/memberFunction.$(MEXENDING): @ns2ClassA/memberFunction.cpp +@ns2ClassA/memberFunction.$(MEXENDING): @ns2ClassA/memberFunction.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @ns2ClassA/memberFunction.cpp -output @ns2ClassA/memberFunction -@ns2ClassA/nsArg.$(MEXENDING): @ns2ClassA/nsArg.cpp +@ns2ClassA/nsArg.$(MEXENDING): @ns2ClassA/nsArg.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @ns2ClassA/nsArg.cpp -output @ns2ClassA/nsArg -@ns2ClassA/nsReturn.$(MEXENDING): @ns2ClassA/nsReturn.cpp +@ns2ClassA/nsReturn.$(MEXENDING): @ns2ClassA/nsReturn.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) @ns2ClassA/nsReturn.cpp -output @ns2ClassA/nsReturn ns2ClassA: new_ns2ClassA_.$(MEXENDING) ns2ClassA_afunction.$(MEXENDING) @ns2ClassA/memberFunction.$(MEXENDING) @ns2ClassA/nsArg.$(MEXENDING) @ns2ClassA/nsReturn.$(MEXENDING) # ns2ns3ClassB -new_ns2ns3ClassB_.$(MEXENDING): new_ns2ns3ClassB_.cpp +new_ns2ns3ClassB_.$(MEXENDING): new_ns2ns3ClassB_.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) new_ns2ns3ClassB_.cpp -output new_ns2ns3ClassB_ ns2ns3ClassB: new_ns2ns3ClassB_.$(MEXENDING) # ns2ClassC -new_ns2ClassC_.$(MEXENDING): new_ns2ClassC_.cpp +new_ns2ClassC_.$(MEXENDING): new_ns2ClassC_.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) new_ns2ClassC_.cpp -output new_ns2ClassC_ ns2ClassC: new_ns2ClassC_.$(MEXENDING) # ClassD -new_ClassD_.$(MEXENDING): new_ClassD_.cpp +new_ClassD_.$(MEXENDING): new_ClassD_.cpp $(PATH_TO_WRAP)/matlab.h $(MEX) $(mex_flags) new_ClassD_.cpp -output new_ClassD_ ClassD: new_ClassD_.$(MEXENDING) diff --git a/wrap/tests/expected_namespaces/new_ClassD_.cpp b/wrap/tests/expected_namespaces/new_ClassD_.cpp index 352c470b0..6ebe46f2d 100644 --- a/wrap/tests/expected_namespaces/new_ClassD_.cpp +++ b/wrap/tests/expected_namespaces/new_ClassD_.cpp @@ -36,7 +36,6 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) self = new Shared(new ClassD()); } collector.insert(self); - std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast (mxGetPr(out[0])) = self; } diff --git a/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp b/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp index 893331034..eca52a3ea 100644 --- a/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp +++ b/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp @@ -36,7 +36,6 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) self = new Shared(new ns1::ClassA()); } collector.insert(self); - std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast (mxGetPr(out[0])) = self; } diff --git a/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp b/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp index 047348d1b..5eb90cae8 100644 --- a/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp +++ b/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp @@ -37,7 +37,6 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) self = new Shared(new ns1::ClassB()); } collector.insert(self); - std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast (mxGetPr(out[0])) = self; } diff --git a/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp b/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp index cb06bf892..f0e6c2036 100644 --- a/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp +++ b/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp @@ -37,7 +37,6 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) self = new Shared(new ns2::ClassA()); } collector.insert(self); - std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast (mxGetPr(out[0])) = self; } diff --git a/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp b/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp index 40c3343ef..ae6a88845 100644 --- a/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp +++ b/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp @@ -36,7 +36,6 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) self = new Shared(new ns2::ClassC()); } collector.insert(self); - std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast (mxGetPr(out[0])) = self; } diff --git a/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp index ace0a261f..bf6e244a2 100644 --- a/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp +++ b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp @@ -37,7 +37,6 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) self = new Shared(new ns2::ns3::ClassB()); } collector.insert(self); - std::cout << "constructed " << self << ", size=" << collector.size() << std::endl; out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast (mxGetPr(out[0])) = self; } diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index cdc6e42d6..8fac80113 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -39,6 +39,10 @@ static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not define typedef vector strvec; +// NOTE: as this path is only used to generate makefiles, it is hardcoded here for testing +// In practice, this path will be an absolute system path, which makes testing it more annoying +static const std::string headerPath = "/not_really_a_real_path/borg/gtsam/wrap"; + /* ************************************************************************* */ TEST( wrap, ArgumentList ) { ArgumentList args; @@ -63,13 +67,13 @@ TEST( wrap, check_exception ) { string path = topdir + "/wrap/tests"; Module module(path.c_str(), "testDependencies",enable_verbose); - CHECK_EXCEPTION(module.matlab_code("mex", "actual_deps", "mexa64", "-O5"), DependencyMissing); + CHECK_EXCEPTION(module.matlab_code("mex", "actual_deps", "mexa64", headerPath, "-O5"), DependencyMissing); } /* ************************************************************************* */ TEST( wrap, parse ) { - string header_path = topdir + "/wrap/tests"; - Module module(header_path.c_str(), "geometry",enable_verbose); + string markup_header_path = topdir + "/wrap/tests"; + Module module(markup_header_path.c_str(), "geometry",enable_verbose); EXPECT_LONGS_EQUAL(3, module.classes.size()); // check using declarations @@ -214,7 +218,7 @@ TEST( wrap, matlab_code_namespaces ) { // emit MATLAB code string exp_path = path + "/tests/expected_namespaces/"; string act_path = "actual_namespaces/"; - module.matlab_code("mex", "actual_namespaces", "mexa64", "-O5"); + module.matlab_code("mex", "actual_namespaces", "mexa64", headerPath, "-O5"); EXPECT(files_equal(exp_path + "new_ClassD_.cpp" , act_path + "new_ClassD_.cpp" )); EXPECT(files_equal(exp_path + "new_ClassD_.m" , act_path + "new_ClassD_.m" )); @@ -255,7 +259,7 @@ TEST( wrap, matlab_code ) { // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make - module.matlab_code("mex", "actual", "mexa64", "-O5"); + module.matlab_code("mex", "actual", "mexa64", headerPath, "-O5"); string epath = path + "/tests/expected/"; string apath = "actual/"; diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index 1b1b631e4..f818094f7 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -29,7 +29,7 @@ using namespace std; * @param interfacePath path to where interface file lives, e.g., borg/gtsam * @param moduleName name of the module to be generated e.g. gtsam * @param toolboxPath path where the toolbox should be generated, e.g. borg/gtsam/build - * @param nameSpace e.g. gtsam + * @param headerPath is the path to matlab.h * @param mexFlags extra arguments for mex script, i.e., include flags etc... */ void generate_matlab_toolbox( @@ -58,6 +58,7 @@ void usage() { cerr << " interfacePath : *absolute* path to directory of module interface file" << endl; cerr << " moduleName : the name of the module, interface file must be called moduleName.h" << endl; cerr << " toolboxPath : the directory in which to generate the wrappers" << endl; + cerr << " headerPath : path to matlab.h" << endl; cerr << " [mexFlags] : extra flags for the mex command" << endl; } From f25f5d5b6d5889a6e6079b72a984a814ca0f6562 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sat, 30 Jun 2012 20:40:03 +0000 Subject: [PATCH 19/21] comments --- gtsam/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 7467ad0cf..50ec1d4de 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -138,6 +138,5 @@ if (GTSAM_BUILD_WRAP) # Macro to handle details of setting up targets # FIXME: issue with dependency between wrap_gtsam and wrap_gtsam_build, only shows up on CMake 2.8.3 - # FIXME: rework install commands to specify only certain filetypes/folders to avoid installing temp files wrap_library(gtsam "${mexFlags}" "../") endif () From 20cf1da5a9befb39cd37b8d8df38931371a4b19a Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sun, 1 Jul 2012 18:00:33 +0000 Subject: [PATCH 20/21] Merge branch 'master' into new_wrap_local --- CppUnitLite/Test.h | 2 +- gtsam/inference/Permutation.h | 15 +- gtsam/inference/VariableIndex.cpp | 11 + gtsam/inference/VariableIndex.h | 15 + gtsam/inference/tests/testVariableIndex.cpp | 4 +- gtsam/linear/VectorValues.cpp | 14 + gtsam/linear/VectorValues.h | 8 + gtsam/nonlinear/ISAM2-impl.cpp | 72 +- gtsam/nonlinear/ISAM2-impl.h | 10 +- gtsam/nonlinear/ISAM2.cpp | 57 +- gtsam/nonlinear/ISAM2.h | 3 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 4 +- gtsam/nonlinear/NonlinearFactorGraph.h | 2 +- tests/testGaussianISAM2.cpp | 861 +++++--------------- tests/testNonlinearFactorGraph.cpp | 4 +- 15 files changed, 380 insertions(+), 702 deletions(-) diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 22566131e..89d96f463 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -110,7 +110,7 @@ protected: { try { condition; \ result_.addFailure (Failure (name_, __FILE__,__LINE__, SimpleString("Didn't throw: ") + StringFrom(#condition))); \ return; } \ - catch (exception_name& e) {} \ + catch (exception_name&) {} \ catch (...) { \ result_.addFailure (Failure (name_, __FILE__,__LINE__, SimpleString("Wrong exception: ") + StringFrom(#condition) + StringFrom(", expected: ") + StringFrom(#exception_name))); \ return; } } diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 3a5b9b0da..39c4447d2 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -133,8 +133,8 @@ public: static Permutation PullToFront(const std::vector& toFront, size_t size, bool filterDuplicates = false); /** - * Create a permutation that pulls the given variables to the front while - * pushing the rest to the back. + * Create a permutation that pushes the given variables to the back while + * pulling the rest to the front. */ static Permutation PushToBack(const std::vector& toBack, size_t size, bool filterDuplicates = false); @@ -154,6 +154,17 @@ public: const_iterator begin() const { return rangeIndices_.begin(); } ///< Iterate through the indices const_iterator end() const { return rangeIndices_.end(); } ///< Iterate through the indices + /** Apply the permutation to a collection, which must have operator[] defined. + * Note that permutable gtsam data structures typically have their own + * permute function to apply a permutation. Permutation::applyToCollection is + * a generic function, e.g. for STL classes. + * @param input The input collection. + * @param output The preallocated output collection, which is assigned output[i] = input[permutation[i]] + */ + template + void applyToCollection(OUTPUT_COLLECTION& output, const INPUT_COLLECTION& input) const { + for(size_t i = 0; i < output.size(); ++i) output[i] = input[(*this)[i]]; } + /// @} /// @name Advanced Interface diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index a29d5e432..4e5eb8649 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -76,4 +76,15 @@ void VariableIndex::permuteInPlace(const Permutation& permutation) { index_.swap(newIndex); } +/* ************************************************************************* */ +void VariableIndex::removeUnusedAtEnd(size_t nToRemove) { +#ifndef NDEBUG + for(size_t i = this->size() - nToRemove; i < this->size(); ++i) + if(!(*this)[i].empty()) + throw std::invalid_argument("Attempting to remove non-empty variables with VariableIndex::removeUnusedAtEnd()"); +#endif + + index_.resize(this->size() - nToRemove); +} + } diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index be47cbd5a..739067dd2 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -119,6 +119,11 @@ public: /** * Remove entries corresponding to the specified factors. + * NOTE: We intentionally do not decrement nFactors_ because the factor + * indices need to remain consistent. Removing factors from a factor graph + * does not shift the indices of other factors. Also, we keep nFactors_ + * one greater than the highest-numbered factor referenced in a VariableIndex. + * * @param indices The indices of the factors to remove, which must match \c factors * @param factors The factors being removed, which must symbolically correspond * exactly to the factors with the specified \c indices that were added. @@ -128,6 +133,12 @@ public: /// Permute the variables in the VariableIndex according to the given permutation void permuteInPlace(const Permutation& permutation); + /** Remove unused empty variables at the end of the ordering (in debug mode + * verifies they are empty). + * @param nToRemove The number of unused variables at the end to remove + */ + void removeUnusedAtEnd(size_t nToRemove); + protected: Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } /// void VariableIndex::remove(const CONTAINER& indices, const FactorGraph& factors) { + // NOTE: We intentionally do not decrement nFactors_ because the factor + // indices need to remain consistent. Removing factors from a factor graph + // does not shift the indices of other factors. Also, we keep nFactors_ + // one greater than the highest-numbered factor referenced in a VariableIndex. for(size_t fi=0; fikeys().size(); ++ji) { diff --git a/gtsam/inference/tests/testVariableIndex.cpp b/gtsam/inference/tests/testVariableIndex.cpp index 7c8e6fa9e..412b3fa04 100644 --- a/gtsam/inference/tests/testVariableIndex.cpp +++ b/gtsam/inference/tests/testVariableIndex.cpp @@ -44,7 +44,9 @@ TEST(VariableIndex, augment) { VariableIndex actual(fg1); actual.augment(fg2); - CHECK(assert_equal(expected, actual)); + LONGS_EQUAL(16, actual.nEntries()); + LONGS_EQUAL(8, actual.nFactors()); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 15925c644..fd7f2757c 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -45,6 +45,14 @@ VectorValues VectorValues::Zero(const VectorValues& x) { return cloned; } +/* ************************************************************************* */ +vector VectorValues::dims() const { + std::vector result(this->size()); + for(Index j = 0; j < this->size(); ++j) + result[j] = this->dim(j); + return result; +} + /* ************************************************************************* */ void VectorValues::insert(Index j, const Vector& value) { // Make sure j does not already exist @@ -188,4 +196,10 @@ VectorValues VectorValues::permute(const Permutation& permutation) const { return lhs; } +/* ************************************************************************* */ +void VectorValues::swap(VectorValues& other) { + this->values_.swap(other.values_); + this->maps_.swap(other.maps_); +} + } \ No newline at end of file diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 463080db9..aa9c1aa6d 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -134,6 +134,9 @@ namespace gtsam { /** Return the summed dimensionality of all variables. */ size_t dim() const { return values_.rows(); } + /** Return the dimension of each vector in this container */ + std::vector dims() const; + /** Check whether a variable with index \c j exists. */ bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; } @@ -296,6 +299,11 @@ namespace gtsam { */ VectorValues permute(const Permutation& permutation) const; + /** + * Swap the data in this VectorValues with another. + */ + void swap(VectorValues& other); + /// @} private: diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 3c373e0ba..5e71ee22b 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -31,7 +31,7 @@ namespace gtsam { void ISAM2::Impl::AddVariables( const Values& newTheta, Values& theta, VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch, vector& replacedKeys, - Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) { + Ordering& ordering, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); @@ -58,9 +58,75 @@ void ISAM2::Impl::AddVariables( assert(ordering.nVars() == delta.size()); assert(ordering.size() == delta.size()); } - assert(ordering.nVars() >= nodes.size()); replacedKeys.resize(ordering.nVars(), false); - nodes.resize(ordering.nVars()); +} + +/* ************************************************************************* */ +void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Clique::shared_ptr& root, + Values& theta, VariableIndex& variableIndex, + VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch, + std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, + GaussianFactorGraph& linearFactors) { + + // Get indices of unused keys + vector unusedIndices; unusedIndices.reserve(unusedKeys.size()); + BOOST_FOREACH(Key key, unusedKeys) { unusedIndices.push_back(ordering[key]); } + + // Create a permutation that shifts the unused variables to the end + Permutation unusedToEnd = Permutation::PushToBack(unusedIndices, delta.size()); + Permutation unusedToEndInverse = *unusedToEnd.inverse(); + + // Use the permutation to remove unused variables while shifting all the others to take up the space + variableIndex.permuteInPlace(unusedToEnd); + variableIndex.removeUnusedAtEnd(unusedIndices.size()); + { + // Create a vector of variable dimensions with the unused ones removed + // by applying the unusedToEnd permutation to the original vector of + // variable dimensions. We only allocate space in the shifted dims + // vector for the used variables, so that the unused ones are dropped + // when the permutation is applied. + vector originalDims = delta.dims(); + vector dims(delta.size() - unusedIndices.size()); + unusedToEnd.applyToCollection(dims, originalDims); + + // Copy from the old data structures to new ones, only iterating up to + // the number of used variables, and applying the unusedToEnd permutation + // in order to remove the unused variables. + VectorValues newDelta(dims); + VectorValues newDeltaNewton(dims); + VectorValues newDeltaGradSearch(dims); + std::vector newReplacedKeys(replacedKeys.size() - unusedIndices.size()); + Base::Nodes newNodes(nodes.size()); // We still keep unused keys at the end until later in ISAM2::recalculate + + for(size_t j = 0; j < dims.size(); ++j) { + newDelta[j] = delta[unusedToEnd[j]]; + newDeltaNewton[j] = deltaNewton[unusedToEnd[j]]; + newDeltaGradSearch[j] = deltaGradSearch[unusedToEnd[j]]; + newReplacedKeys[j] = replacedKeys[unusedToEnd[j]]; + } + + // Permute the nodes index so the unused variables are the end + unusedToEnd.applyToCollection(newNodes, nodes); + + // Swap the new data structures with the outputs of this function + delta.swap(newDelta); + deltaNewton.swap(newDeltaNewton); + deltaGradSearch.swap(newDeltaGradSearch); + replacedKeys.swap(newReplacedKeys); + nodes.swap(newNodes); + } + + // Reorder and remove from ordering and solution + ordering.permuteWithInverse(unusedToEndInverse); + BOOST_REVERSE_FOREACH(Key key, unusedKeys) { + ordering.pop_back(key); + theta.erase(key); + } + + // Finally, permute references to variables + if(root) + root->permuteWithInverse(unusedToEndInverse); + linearFactors.permuteWithInverse(unusedToEndInverse); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index fdb39d855..b873c87bf 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -48,8 +48,16 @@ struct ISAM2::Impl { */ static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch, std::vector& replacedKeys, - Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + Ordering& ordering, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + /** + * Remove variables from the ISAM2 system. + */ + static void RemoveVariables(const FastSet& unusedKeys, const ISAM2Clique::shared_ptr& root, + Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, + VectorValues& deltaGradSearch, std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, + GaussianFactorGraph& linearFactors); + /** * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol * in each NonlinearFactor, obtains the index by calling ordering[symbol]. diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 0c3c198aa..e47127edd 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -249,6 +249,9 @@ boost::shared_ptr > ISAM2::recalculate( this->removeTop(markedKeys, affectedBayesNet, orphans); toc(1, "removetop"); + // Now that the top is removed, correct the size of the Nodes index + this->nodes_.resize(delta_.size()); + if(debug) affectedBayesNet.print("Removed top: "); if(debug) orphans.print("Orphans: "); @@ -266,6 +269,14 @@ boost::shared_ptr > ISAM2::recalculate( // ordering provides all keys in conditionals, there cannot be others because path to root included tic(2,"affectedKeys"); FastList affectedKeys = affectedBayesNet.ordering(); + // The removed top also contained removed variables, which will be ordered + // higher than the number of variables in the system since unused variables + // were already removed in ISAM2::update. + for(FastList::iterator key = affectedKeys.begin(); key != affectedKeys.end(); ) + if(*key >= delta_.size()) + affectedKeys.erase(key++); + else + ++key; toc(2,"affectedKeys"); boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result @@ -539,15 +550,46 @@ ISAM2Result ISAM2::update( BOOST_FOREACH(size_t index, removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); + if(params_.cacheLinearizedFactors) + linearFactors_.remove(index); } // Remove removed factors from the variable index so we do not attempt to relinearize them variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_)); + + // We now need to start keeping track of the marked keys involved in added or + // removed factors. + FastSet markedKeys; + + // Remove unused keys and add keys from removed factors that are still used + // in other factors to markedKeys. + { + // Get keys from removed factors + FastSet removedFactorSymbKeys = removeFactors.keys(); + + // For each key, if still used in other factors, add to markedKeys to be + // recalculated, or if not used, add to unusedKeys to be removed from the + // system. Note that unusedKeys stores Key while markedKeys stores Index. + FastSet unusedKeys; + BOOST_FOREACH(Key key, removedFactorSymbKeys) { + Index index = ordering_[key]; + if(variableIndex_[index].empty()) + unusedKeys.insert(key); + else + markedKeys.insert(index); + } + + // Remove unused keys. We must hold on to the new nodes index for now + // instead of placing it into the tree because removeTop will need to + // update it. + Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, + deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_); + } toc(1,"push_back factors"); tic(2,"add new variables"); // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. - Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_, Base::nodes_); + Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_); // New keys for detailed results if(params_.enableDetailedResults) { inverseOrdering_ = ordering_.invert(); @@ -561,13 +603,12 @@ ISAM2Result ISAM2::update( tic(4,"gather involved keys"); // 3. Mark linear update - FastSet markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors - // Also mark keys involved in removed factors - { - FastSet markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors - markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys - } - // Observed keys for detailed results + { + FastSet newFactorIndices = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors + markedKeys.insert(newFactorIndices.begin(), newFactorIndices.end()); + } + + // Observed keys for detailed results if(params_.enableDetailedResults) { BOOST_FOREACH(Index index, markedKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 2fd3e0b23..1fa893819 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -516,8 +516,7 @@ private: GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, - const FastVector& observedKeys, - const boost::optional >& constrainKeys, ISAM2Result& result); + const FastVector& observedKeys, const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); void updateDelta(bool forceFullSolve = false) const; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 8c4076c56..969e40677 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -54,8 +54,8 @@ double NonlinearFactorGraph::error(const Values& c) const { } /* ************************************************************************* */ -std::set NonlinearFactorGraph::keys() const { - std::set keys; +FastSet NonlinearFactorGraph::keys() const { + FastSet keys; BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) keys.insert(factor->begin(), factor->end()); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 1742c7b78..85cc0e63f 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -47,7 +47,7 @@ namespace gtsam { void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** return keys as an ordered set - ordering is by key value */ - std::set keys() const; + FastSet keys() const; /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */ double error(const Values& c) const; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 994077777..f63d2add5 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -43,7 +44,6 @@ ISAM2 createSlamlikeISAM2( // These variables will be reused and accumulate factors and values ISAM2 isam(params); -// ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); Values fullinit; planarSLAM::Graph fullgraph; @@ -135,7 +135,7 @@ ISAM2 createSlamlikeISAM2( } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, AddVariables) { +TEST_UNSAFE(ISAM2, ImplAddVariables) { // Create initial state Values theta; @@ -160,8 +160,6 @@ TEST_UNSAFE(ISAM2, AddVariables) { Ordering ordering; ordering += 100, 0; - ISAM2::Nodes nodes(2); - // Verify initial state LONGS_EQUAL(0, ordering[100]); LONGS_EQUAL(1, ordering[0]); @@ -193,10 +191,8 @@ TEST_UNSAFE(ISAM2, AddVariables) { Ordering orderingExpected; orderingExpected += 100, 0, 1; - ISAM2::Nodes nodesExpected(3, ISAM2::sharedClique()); - // Expand initial state - ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes); + ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering); EXPECT(assert_equal(thetaExpected, theta)); EXPECT(assert_equal(deltaExpected, delta)); @@ -205,6 +201,95 @@ TEST_UNSAFE(ISAM2, AddVariables) { EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); EXPECT(assert_equal(orderingExpected, ordering)); } +/* ************************************************************************* */ +TEST_UNSAFE(ISAM2, ImplRemoveVariables) { + + // Create initial state + Values theta; + theta.insert(0, Pose2(.1, .2, .3)); + theta.insert(1, Pose2(.6, .7, .8)); + theta.insert(100, Point2(.4, .5)); + + SymbolicFactorGraph sfg; + sfg.push_back(boost::make_shared(Index(0), Index(2))); + sfg.push_back(boost::make_shared(Index(0), Index(1))); + VariableIndex variableIndex(sfg); + + VectorValues delta; + delta.insert(0, Vector_(3, .1, .2, .3)); + delta.insert(1, Vector_(3, .4, .5, .6)); + delta.insert(2, Vector_(2, .7, .8)); + + VectorValues deltaNewton; + deltaNewton.insert(0, Vector_(3, .1, .2, .3)); + deltaNewton.insert(1, Vector_(3, .4, .5, .6)); + deltaNewton.insert(2, Vector_(2, .7, .8)); + + VectorValues deltaRg; + deltaRg.insert(0, Vector_(3, .1, .2, .3)); + deltaRg.insert(1, Vector_(3, .4, .5, .6)); + deltaRg.insert(2, Vector_(2, .7, .8)); + + vector replacedKeys(3, false); + replacedKeys[0] = true; + replacedKeys[1] = false; + replacedKeys[2] = true; + + Ordering ordering; ordering += 100, 1, 0; + + ISAM2::Nodes nodes(3); + + // Verify initial state + LONGS_EQUAL(0, ordering[100]); + LONGS_EQUAL(1, ordering[1]); + LONGS_EQUAL(2, ordering[0]); + + // Create expected state + Values thetaExpected; + thetaExpected.insert(0, Pose2(.1, .2, .3)); + thetaExpected.insert(100, Point2(.4, .5)); + + SymbolicFactorGraph sfgRemoved; + sfgRemoved.push_back(boost::make_shared(Index(0), Index(1))); + sfgRemoved.push_back(SymbolicFactorGraph::sharedFactor()); // Add empty factor to keep factor indices consistent + VariableIndex variableIndexExpected(sfgRemoved); + + VectorValues deltaExpected; + deltaExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaExpected.insert(1, Vector_(2, .7, .8)); + + VectorValues deltaNewtonExpected; + deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaNewtonExpected.insert(1, Vector_(2, .7, .8)); + + VectorValues deltaRgExpected; + deltaRgExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaRgExpected.insert(1, Vector_(2, .7, .8)); + + vector replacedKeysExpected(2, true); + + Ordering orderingExpected; orderingExpected += 100, 0; + + ISAM2::Nodes nodesExpected(2); + + // Reduce initial state + FastSet unusedKeys; + unusedKeys.insert(1); + vector removedFactorsI; removedFactorsI.push_back(1); + SymbolicFactorGraph removedFactors; removedFactors.push_back(sfg[1]); + variableIndex.remove(removedFactorsI, removedFactors); + GaussianFactorGraph linearFactors; + ISAM2::Impl::RemoveVariables(unusedKeys, ISAM2::sharedClique(), theta, variableIndex, delta, deltaNewton, deltaRg, + replacedKeys, ordering, nodes, linearFactors); + + EXPECT(assert_equal(thetaExpected, theta)); + EXPECT(assert_equal(variableIndexExpected, variableIndex)); + EXPECT(assert_equal(deltaExpected, delta)); + EXPECT(assert_equal(deltaNewtonExpected, deltaNewton)); + EXPECT(assert_equal(deltaRgExpected, deltaRg)); + EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); + EXPECT(assert_equal(orderingExpected, ordering)); +} /* ************************************************************************* */ //TEST(ISAM2, IndicesFromFactors) { @@ -293,7 +378,11 @@ TEST(ISAM2, optimize2) { } /* ************************************************************************* */ -bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam) { +bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { + + TestResult& result_ = result; + const SimpleString name_ = test.getName(); + Values actual = isam.calculateEstimate(); Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first; GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); @@ -303,488 +392,94 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, cons VectorValues delta = optimize(gbn); Values expected = fullinit.retract(delta, ordering); - return assert_equal(expected, actual); + bool isamEqual = assert_equal(expected, actual); + + // The following two checks make sure that the cached gradients are maintained and used correctly + + // Check gradient at each node + bool nodeGradientsOk = true; + typedef ISAM2::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { + // Compute expected gradient + FactorGraph jfg; + jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); + VectorValues expectedGradient(*allocateVectorValues(isam)); + gradientAtZero(jfg, expectedGradient); + // Compare with actual gradients + int variablePosition = 0; + for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { + const int dim = clique->conditional()->dim(jit); + Vector actual = clique->gradientContribution().segment(variablePosition, dim); + bool gradOk = assert_equal(expectedGradient[*jit], actual); + EXPECT(gradOk); + nodeGradientsOk = nodeGradientsOk && gradOk; + variablePosition += dim; + } + bool dimOk = clique->gradientContribution().rows() == variablePosition; + EXPECT(dimOk); + nodeGradientsOk = nodeGradientsOk && dimOk; + } + + // Check gradient + VectorValues expectedGradient(*allocateVectorValues(isam)); + gradientAtZero(FactorGraph(isam), expectedGradient); + VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); + VectorValues actualGradient(*allocateVectorValues(isam)); + gradientAtZero(isam, actualGradient); + bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient); + EXPECT(expectedGradOk); + bool totalGradOk = assert_equal(expectedGradient, actualGradient); + EXPECT(totalGradOk); + + return nodeGradientsOk && expectedGradOk && totalGradOk; } /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_gaussnewton) { - // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; - - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init); - ++ i; - } + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_dogleg) { // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; - - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init); - ++ i; - } + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_gaussnewton_qr) { // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR)); Values fullinit; planarSLAM::Graph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR)); - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init); - ++ i; - } - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_dogleg_qr) { // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR)); Values fullinit; planarSLAM::Graph fullgraph; - - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init); - ++ i; - } + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR)); // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ @@ -888,127 +583,43 @@ TEST(ISAM2, removeFactors) // then removes the 2nd-to-last landmark measurement // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - // i keeps track of the time step - size_t i = 0; + // Remove the 2nd measurement on landmark 0 (Key 100) + FastVector toRemove; + toRemove.push_back(12); + isam.update(planarSLAM::Graph(), Values(), toRemove); - // Add a prior at time 0 and update isam - { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors[0]); - fullgraph.push_back(newfactors[2]); // Don't add measurement on landmark 0 - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - ISAM2Result result = isam.update(newfactors, init); - ++ i; - - // Remove the measurement on landmark 0 - FastVector toRemove; - EXPECT_LONGS_EQUAL(isam.getFactorsUnsafe().size()-2, result.newFactorsIndices[1]); - toRemove.push_back(result.newFactorsIndices[1]); - isam.update(planarSLAM::Graph(), Values(), toRemove); - } + // Remove the factor from the full system + fullgraph.remove(12); // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } +/* ************************************************************************* */ +TEST_UNSAFE(ISAM2, removeVariables) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + planarSLAM::Graph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); + // Remove the measurement on landmark 0 (Key 100) + FastVector toRemove; + toRemove.push_back(7); + toRemove.push_back(14); + isam.update(planarSLAM::Graph(), Values(), toRemove); + + // Remove the factors and variable from the full system + fullgraph.remove(7); + fullgraph.remove(14); + fullinit.erase(100); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ @@ -1037,7 +648,7 @@ TEST_UNSAFE(ISAM2, swapFactors) // Compare solutions EXPECT(assert_equal(fullgraph, planarSLAM::Graph(isam.getFactorsUnsafe()))); - EXPECT(isam_check(fullgraph, fullinit, isam)); + EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); // Check gradient at each node typedef ISAM2::sharedClique sharedClique; @@ -1097,7 +708,7 @@ TEST(ISAM2, constrained_ordering) isam.update(newfactors, init); } - CHECK(isam_check(fullgraph, fullinit, isam)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); // Add odometry from time 0 to time 5 for( ; i<5; ++i) { @@ -1165,7 +776,7 @@ TEST(ISAM2, constrained_ordering) } // Compare solutions - EXPECT(isam_check(fullgraph, fullinit, isam)); + EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); // Check that x3 and x4 are last, but either can come before the other EXPECT(isam.getOrdering()[(3)] == 12 && isam.getOrdering()[(4)] == 13); @@ -1204,122 +815,14 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check) { // These variables will be reused and accumulate factors and values - ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); - params.enablePartialRelinearizationCheck = true; - ISAM2 isam(params); Values fullinit; planarSLAM::Graph fullgraph; - - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init); - ++ i; - } + ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); + params.enablePartialRelinearizationCheck = true; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params); // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index f44113f93..f677eb0dc 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -65,9 +65,9 @@ TEST( Graph, error ) TEST( Graph, keys ) { Graph fg = createNonlinearFactorGraph(); - set actual = fg.keys(); + FastSet actual = fg.keys(); LONGS_EQUAL(3, actual.size()); - set::const_iterator it = actual.begin(); + FastSet::const_iterator it = actual.begin(); LONGS_EQUAL(L(1), *(it++)); LONGS_EQUAL(X(1), *(it++)); LONGS_EQUAL(X(2), *(it++)); From 9273c3d18cdec741cbdd287d93c93eb87bc76365 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sun, 1 Jul 2012 18:00:35 +0000 Subject: [PATCH 21/21] Added minimal output to wrap generation --- wrap/FileWriter.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/wrap/FileWriter.cpp b/wrap/FileWriter.cpp index ae474be0a..38abf2488 100644 --- a/wrap/FileWriter.cpp +++ b/wrap/FileWriter.cpp @@ -46,6 +46,10 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const { ofs << new_contents; ofs.close(); if (verbose_) cerr << " ...complete" << endl; + + // Add small message whenever writing a new file and not running in full verbose mode + if (!verbose_) + cout << "wrap: generating " << filename_ << endl; } else { if (verbose_) cerr << " ...no update" << endl; }