From e6a43d6330f5fe1ca43e41b0c49aeca31b4f5a32 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 12 Nov 2011 21:19:46 +0000 Subject: [PATCH 02/17] (in branch) Dogleg in ISAM2 in progress --- gtsam/inference/BayesTree-inl.h | 170 ++++++------ gtsam/inference/BayesTree.h | 243 ++++++++++-------- gtsam/inference/FactorGraph-inl.h | 15 +- gtsam/inference/FactorGraph.h | 6 +- .../inference/GenericMultifrontalSolver-inl.h | 6 +- gtsam/inference/GenericMultifrontalSolver.h | 2 +- gtsam/inference/JunctionTree-inl.h | 56 ++-- gtsam/inference/JunctionTree.h | 13 +- gtsam/linear/GaussianFactorGraph.cpp | 3 - gtsam/linear/GaussianFactorGraph.h | 3 +- gtsam/linear/GaussianJunctionTree.cpp | 10 +- gtsam/linear/GaussianJunctionTree.h | 14 +- gtsam/nonlinear/ISAM2-impl-inl.h | 12 +- gtsam/nonlinear/ISAM2-inl.h | 14 +- gtsam/nonlinear/ISAM2.h | 90 ++++++- tests/testGaussianISAM2.cpp | 2 +- 16 files changed, 383 insertions(+), 276 deletions(-) diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index fce5c3e19..c63553ac2 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -44,7 +44,7 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTree::Clique::assertInvariants() const { + void BayesTreeClique::assertInvariants() const { #ifndef NDEBUG // We rely on the keys being sorted // FastVector sortedUniqueKeys(conditional_->begin(), conditional_->end()); @@ -57,19 +57,19 @@ namespace gtsam { /* ************************************************************************* */ template - BayesTree::Clique::Clique(const sharedConditional& conditional) : conditional_(conditional) { + BayesTreeClique::BayesTreeClique(const sharedConditional& conditional) : conditional_(conditional) { assertInvariants(); } /* ************************************************************************* */ template - void BayesTree::Clique::print(const string& s) const { + void BayesTreeClique::print(const string& s) const { conditional_->print(s); } /* ************************************************************************* */ template - size_t BayesTree::Clique::treeSize() const { + size_t BayesTreeClique::treeSize() const { size_t size = 1; BOOST_FOREACH(const shared_ptr& child, children_) size += child->treeSize(); @@ -78,7 +78,7 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTree::Clique::printTree(const string& indent) const { + void BayesTreeClique::printTree(const string& indent) const { print(indent); BOOST_FOREACH(const shared_ptr& child, children_) child->printTree(indent+" "); @@ -86,9 +86,8 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTree::Clique::permuteWithInverse(const Permutation& inversePermutation) { + void BayesTreeClique::permuteWithInverse(const Permutation& inversePermutation) { conditional_->permuteWithInverse(inversePermutation); - if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); BOOST_FOREACH(const shared_ptr& child, children_) { child->permuteWithInverse(inversePermutation); } @@ -97,7 +96,7 @@ namespace gtsam { /* ************************************************************************* */ template - bool BayesTree::Clique::permuteSeparatorWithInverse(const Permutation& inversePermutation) { + bool BayesTreeClique::permuteSeparatorWithInverse(const Permutation& inversePermutation) { bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation); #ifndef NDEBUG if(!changed) { @@ -108,7 +107,6 @@ namespace gtsam { } #endif if(changed) { - if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); BOOST_FOREACH(const shared_ptr& child, children_) { (void)child->permuteSeparatorWithInverse(inversePermutation); } @@ -118,17 +116,17 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename BayesTree::CliqueData - BayesTree::getCliqueData() const { + template + typename BayesTree::CliqueData + BayesTree::getCliqueData() const { CliqueData data; getCliqueData(data, root_); return data; } - template - void BayesTree::getCliqueData(CliqueData& data, - BayesTree::sharedClique clique) const { + template + void BayesTree::getCliqueData(CliqueData& data, + BayesTree::sharedClique clique) const { data.conditionalSizes.push_back((*clique)->nrFrontals()); data.separatorSizes.push_back((*clique)->nrParents()); BOOST_FOREACH(sharedClique c, clique->children_) { @@ -137,8 +135,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::saveGraph(const std::string &s) const { + template + void BayesTree::saveGraph(const std::string &s) const { if (!root_.get()) throw invalid_argument("the root of bayes tree has not been initialized!"); ofstream of(s.c_str()); of<< "digraph G{\n"; @@ -147,9 +145,9 @@ namespace gtsam { of.close(); } - template - void BayesTree::saveGraph(ostream &s, - BayesTree::sharedClique clique, + template + void BayesTree::saveGraph(ostream &s, + BayesTree::sharedClique clique, int parentnum) const { static int num = 0; bool first = true; @@ -184,9 +182,9 @@ namespace gtsam { } - template - typename BayesTree::CliqueStats - BayesTree::CliqueData::getStats() const { + template + typename BayesTree::CliqueStats + BayesTree::CliqueData::getStats() const { CliqueStats stats; double sum = 0.0; @@ -216,7 +214,7 @@ namespace gtsam { // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p /* ************************************************************************* */ template - BayesNet BayesTree::Clique::shortcut(shared_ptr R, + BayesNet BayesTreeClique::shortcut(shared_ptr R, Eliminate function) { static const bool debug = false; @@ -224,7 +222,7 @@ namespace gtsam { // A first base case is when this clique or its parent is the root, // in which case we return an empty Bayes net. - sharedClique parent(parent_.lock()); + shared_ptr parent(parent_.lock()); if (R.get()==this || parent==R) { BayesNet empty; @@ -318,7 +316,7 @@ namespace gtsam { // Because the root clique could be very big. /* ************************************************************************* */ template - FactorGraph BayesTree::Clique::marginal( + FactorGraph BayesTreeClique::marginal( shared_ptr R, Eliminate function) { // If we are the root, just return this root // NOTE: immediately cast to a factor graph @@ -339,7 +337,7 @@ namespace gtsam { // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R) /* ************************************************************************* */ template - FactorGraph BayesTree::Clique::joint( + FactorGraph BayesTreeClique::joint( shared_ptr C2, shared_ptr R, Eliminate function) { // For now, assume neither is the root @@ -367,23 +365,23 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::Cliques::print(const std::string& s) const { + template + void BayesTree::Cliques::print(const std::string& s) const { cout << s << ":\n"; BOOST_FOREACH(sharedClique clique, *this) clique->printTree(); } /* ************************************************************************* */ - template - bool BayesTree::Cliques::equals(const Cliques& other, double tol) const { + template + bool BayesTree::Cliques::equals(const Cliques& other, double tol) const { return other == *this; } /* ************************************************************************* */ - template - typename BayesTree::sharedClique BayesTree::addClique( - const sharedConditional& conditional, sharedClique parent_clique) { + template + typename BayesTree::sharedClique + BayesTree::addClique(const sharedConditional& conditional, sharedClique parent_clique) { sharedClique new_clique(new Clique(conditional)); nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size())); BOOST_FOREACH(Index key, conditional->frontals()) @@ -397,8 +395,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename BayesTree::sharedClique BayesTree::addClique( + template + typename BayesTree::sharedClique BayesTree::addClique( const sharedConditional& conditional, list& child_cliques) { sharedClique new_clique(new Clique(conditional)); nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size())); @@ -412,8 +410,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - inline void BayesTree::addToCliqueFront(BayesTree& bayesTree, const sharedConditional& conditional, const sharedClique& clique) { + template + inline void BayesTree::addToCliqueFront(BayesTree& bayesTree, const sharedConditional& conditional, const sharedClique& clique) { static const bool debug = false; #ifndef NDEBUG // Debug check to make sure the conditional variable is ordered lower than @@ -439,8 +437,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::removeClique(sharedClique clique) { + template + void BayesTree::removeClique(sharedClique clique) { if (clique->isRoot()) root_.reset(); @@ -449,7 +447,7 @@ namespace gtsam { // orphan my children BOOST_FOREACH(sharedClique child, clique->children_) - child->parent_ = typename BayesTree::Clique::weak_ptr(); + child->parent_ = typename Clique::weak_ptr(); BOOST_FOREACH(Index key, (*clique->conditional())) { nodes_[key].reset(); @@ -457,27 +455,28 @@ namespace gtsam { } /* ************************************************************************* */ - template - BayesTree::BayesTree() { + template + BayesTree::BayesTree() { } /* ************************************************************************* */ - template - BayesTree::BayesTree(const BayesNet& bayesNet) { + template + BayesTree::BayesTree(const BayesNet& bayesNet) { typename BayesNet::const_reverse_iterator rit; for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) insert(*this, *rit); } /* ************************************************************************* */ - template - BayesTree::BayesTree(const BayesNet& bayesNet, std::list > subtrees) { + template + BayesTree::BayesTree(const BayesNet& bayesNet, std::list > subtrees) { if (bayesNet.size() == 0) throw invalid_argument("BayesTree::insert: empty bayes net!"); // get the roots of child subtrees and merge their nodes_ list childRoots; - BOOST_FOREACH(const BayesTree& subtree, subtrees) { + typedef BayesTree Tree; + BOOST_FOREACH(const Tree& subtree, subtrees) { nodes_.insert(subtree.nodes_.begin(), subtree.nodes_.end()); childRoots.push_back(subtree.root()); } @@ -496,8 +495,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::print(const string& s) const { + template + void BayesTree::print(const string& s) const { if (root_.use_count() == 0) { printf("WARNING: BayesTree.print encountered a forest...\n"); return; @@ -509,26 +508,26 @@ namespace gtsam { /* ************************************************************************* */ // binary predicate to test equality of a pair for use in equals - template + template bool check_sharedCliques( - const typename BayesTree::sharedClique& v1, - const typename BayesTree::sharedClique& v2 + const typename BayesTree::sharedClique& v1, + const typename BayesTree::sharedClique& v2 ) { return v1->equals(*v2); } /* ************************************************************************* */ - template - bool BayesTree::equals(const BayesTree& other, + template + bool BayesTree::equals(const BayesTree& other, double tol) const { return size()==other.size() && - std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques); + std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques); } /* ************************************************************************* */ - template + template template - inline Index BayesTree::findParentClique(const CONTAINER& parents) const { + inline Index BayesTree::findParentClique(const CONTAINER& parents) const { typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); assert(lowestOrderedParent != parents.end()); return *lowestOrderedParent; @@ -548,8 +547,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::insert(BayesTree& bayesTree, const sharedConditional& conditional) + template + void BayesTree::insert(BayesTree& bayesTree, const sharedConditional& conditional) { static const bool debug = false; @@ -597,8 +596,8 @@ namespace gtsam { /* ************************************************************************* */ //TODO: remove this function after removing TSAM.cpp - template - typename BayesTree::sharedClique BayesTree::insert( + template + typename BayesTree::sharedClique BayesTree::insert( const sharedConditional& clique, list& children, bool isRootClique) { if (clique->nrFrontals() == 0) @@ -612,18 +611,19 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::fillNodesIndex(const sharedClique& subtree) { + template + void BayesTree::fillNodesIndex(const sharedClique& subtree) { // Add each frontal variable of this root node BOOST_FOREACH(const Index& key, subtree->conditional()->frontals()) { nodes_[key] = subtree; } // Fill index for each child - BOOST_FOREACH(const typename BayesTree::sharedClique& child, subtree->children_) { + typedef typename BayesTree::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& child, subtree->children_) { fillNodesIndex(child); } } /* ************************************************************************* */ - template - void BayesTree::insert(const sharedClique& subtree) { + template + void BayesTree::insert(const sharedClique& subtree) { if(subtree) { // Find the parent clique of the new subtree. By the running intersection // property, those separator variables in the subtree that are ordered @@ -651,8 +651,8 @@ namespace gtsam { /* ************************************************************************* */ // First finds clique marginal then marginalizes that /* ************************************************************************* */ - template - typename CONDITIONAL::FactorType::shared_ptr BayesTree::marginalFactor( + template + typename CONDITIONAL::FactorType::shared_ptr BayesTree::marginalFactor( Index key, Eliminate function) const { // get clique containing key @@ -666,8 +666,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename BayesNet::shared_ptr BayesTree::marginalBayesNet( + template + typename BayesNet::shared_ptr BayesTree::marginalBayesNet( Index key, Eliminate function) const { // calculate marginal as a factor graph @@ -681,9 +681,9 @@ namespace gtsam { /* ************************************************************************* */ // Find two cliques, their joint, then marginalizes /* ************************************************************************* */ - template - typename FactorGraph::shared_ptr BayesTree< - CONDITIONAL>::joint(Index key1, Index key2, Eliminate function) const { + template + typename FactorGraph::shared_ptr + BayesTree::joint(Index key1, Index key2, Eliminate function) const { // get clique C1 and C2 sharedClique C1 = (*this)[key1], C2 = (*this)[key2]; @@ -698,8 +698,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename BayesNet::shared_ptr BayesTree::jointBayesNet( + template + typename BayesNet::shared_ptr BayesTree::jointBayesNet( Index key1, Index key2, Eliminate function) const { // eliminate factor graph marginal to a Bayes net @@ -708,17 +708,17 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::clear() { + template + void BayesTree::clear() { // Remove all nodes and clear the root pointer nodes_.clear(); root_.reset(); } /* ************************************************************************* */ - template - void BayesTree::removePath(sharedClique clique, - BayesNet& bn, typename BayesTree::Cliques& orphans) { + template + void BayesTree::removePath(sharedClique clique, + BayesNet& bn, typename BayesTree::Cliques& orphans) { // base case is NULL, if so we do nothing and return empties above if (clique!=NULL) { @@ -730,7 +730,7 @@ namespace gtsam { this->removeClique(clique); // remove path above me - this->removePath(clique->parent_.lock(), bn, orphans); + this->removePath(sharedClique(clique->parent_.lock()), bn, orphans); // add children to list of orphans (splice also removed them from clique->children_) orphans.splice (orphans.begin(), clique->children_); @@ -741,10 +741,10 @@ namespace gtsam { } /* ************************************************************************* */ - template + template template - void BayesTree::removeTop(const CONTAINER& keys, - BayesNet& bn, typename BayesTree::Cliques& orphans) { + void BayesTree::removeTop(const CONTAINER& keys, + BayesNet& bn, typename BayesTree::Cliques& orphans) { // process each key of the new factor BOOST_FOREACH(const Index& key, keys) { diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index ebe59d135..eeebf45bc 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -33,14 +33,19 @@ namespace gtsam { + // Forward declaration of BayesTreeClique which is defined below BayesTree in this file + template class BayesTreeClique; + /** * Bayes tree - * Templated on the CONDITIONAL class, the type of node in the underlying Bayes chain. - * This could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional + * @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain, + * which could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional. + * @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this + * as it is only used when developing special versions of BayesTree, e.g. for ISAM2. * * \ingroup Multifrontal */ - template + template > class BayesTree { public: @@ -52,100 +57,7 @@ namespace gtsam { typedef typename CONDITIONAL::FactorType FactorType; typedef typename FactorGraph::Eliminate Eliminate; - /** - * A Clique in the tree is an incomplete Bayes net: the variables - * in the Bayes net are the frontal nodes, and the variables conditioned - * on are the separator. We also have pointers up and down the tree. - * - * Since our Conditional class already handles multiple frontal variables, - * this Clique contains exactly 1 conditional. - */ - struct Clique { - - protected: - void assertInvariants() const; - - public: - typedef CONDITIONAL ConditionalType; - typedef typename boost::shared_ptr shared_ptr; - typedef typename boost::weak_ptr weak_ptr; - sharedConditional conditional_; - weak_ptr parent_; - std::list children_; - typename FactorType::shared_ptr cachedFactor_; - - friend class BayesTree; - - //* Constructor */ - Clique() {} - - Clique(const sharedConditional& conditional); - - /** print this node */ - void print(const std::string& s = "") const; - - /** The arrow operator accesses the conditional */ - const CONDITIONAL* operator->() const { return conditional_.get(); } - - /** The arrow operator accesses the conditional */ - CONDITIONAL* operator->() { return conditional_.get(); } - - /** Access the conditional */ - const sharedConditional& conditional() const { return conditional_; } - - /** is this the root of a Bayes tree ? */ - inline bool isRoot() const { return parent_.expired(); } - - /** return the const reference of children */ - std::list& children() { return children_; } - const std::list& children() const { return children_; } - - /** The size of subtree rooted at this clique, i.e., nr of Cliques */ - size_t treeSize() const; - - /** Access the cached factor (this is a hack) */ - typename FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } - - /** print this node and entire subtree below it */ - void printTree(const std::string& indent="") const; - - /** Permute the variables in the whole subtree rooted at this clique */ - void permuteWithInverse(const Permutation& inversePermutation); - - /** Permute variables when they only appear in the separators. In this - * case the running intersection property will be used to prevent always - * traversing the whole tree. Returns whether any separator variables in - * this subtree were reordered. - */ - bool permuteSeparatorWithInverse(const Permutation& inversePermutation); - - /** return the conditional P(S|Root) on the separator given the root */ - // TODO: create a cached version - BayesNet shortcut(shared_ptr root, Eliminate function); - - /** return the marginal P(C) of the clique */ - FactorGraph marginal(shared_ptr root, Eliminate function); - - /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ - FactorGraph joint(shared_ptr C2, shared_ptr root, Eliminate function); - - bool equals(const Clique& other, double tol=1e-9) const { - return (!conditional_ && !other.conditional()) || - conditional_->equals(*(other.conditional()), tol); - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(conditional_); - ar & BOOST_SERIALIZATION_NVP(parent_); - ar & BOOST_SERIALIZATION_NVP(children_); - ar & BOOST_SERIALIZATION_NVP(cachedFactor_); - } - - }; // \struct Clique + typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique // typedef for shared pointers to cliques typedef boost::shared_ptr sharedClique; @@ -207,7 +119,7 @@ namespace gtsam { * parents are already in the clique or its separators. This function does * not check for this condition, it just updates the data structures. */ - static void addToCliqueFront(BayesTree& bayesTree, + static void addToCliqueFront(BayesTree& bayesTree, const sharedConditional& conditional, const sharedClique& clique); /** Fill the nodes index for a subtree */ @@ -225,7 +137,7 @@ namespace gtsam { * Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the * new root clique and the subtrees are connected to the root clique. */ - BayesTree(const BayesNet& bayesNet, std::list > subtrees); + BayesTree(const BayesNet& bayesNet, std::list > subtrees); /** Destructor */ virtual ~BayesTree() {} @@ -238,7 +150,7 @@ namespace gtsam { * This function only applies for Symbolic case with IndexCondtional, * We make it static so that it won't be compiled in GaussianConditional case. * */ - static void insert(BayesTree& bayesTree, const sharedConditional& conditional); + static void insert(BayesTree& bayesTree, const sharedConditional& conditional); /** * Insert a new clique corresponding to the given Bayes net. @@ -262,7 +174,7 @@ namespace gtsam { */ /** check equality */ - bool equals(const BayesTree& other, double tol = 1e-9) const; + bool equals(const BayesTree& other, double tol = 1e-9) const; /** * Find parent clique of a conditional. It will look at all parents and @@ -357,10 +269,10 @@ namespace gtsam { /* ************************************************************************* */ - template + template void _BayesTree_dim_adder( std::vector& dims, - const typename BayesTree::sharedClique& clique) { + const typename BayesTree::sharedClique& clique) { if(clique) { // Add dims from this clique @@ -368,18 +280,133 @@ namespace gtsam { dims[*it] = (*clique)->dim(it); // Traverse children - BOOST_FOREACH(const typename BayesTree::sharedClique& child, clique->children()) { - _BayesTree_dim_adder(dims, child); + typedef typename BayesTree::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& child, clique->children()) { + _BayesTree_dim_adder(dims, child); } } } /* ************************************************************************* */ - template - boost::shared_ptr allocateVectorValues(const BayesTree& bt) { + template + boost::shared_ptr allocateVectorValues(const BayesTree& bt) { std::vector dimensions(bt.nodes().size(), 0); - _BayesTree_dim_adder(dimensions, bt.root()); + _BayesTree_dim_adder(dimensions, bt.root()); return boost::shared_ptr(new VectorValues(dimensions)); } + + /* ************************************************************************* */ + /** + * A Clique in the tree is an incomplete Bayes net: the variables + * in the Bayes net are the frontal nodes, and the variables conditioned + * on are the separator. We also have pointers up and down the tree. + * + * Since our Conditional class already handles multiple frontal variables, + * this Clique contains exactly 1 conditional. + */ + template + struct BayesTreeClique { + + protected: + void assertInvariants() const; + + public: + typedef BayesTreeClique This; + typedef CONDITIONAL ConditionalType; + typedef boost::shared_ptr sharedConditional; + typedef typename boost::shared_ptr shared_ptr; + typedef typename boost::weak_ptr weak_ptr; + typedef typename CONDITIONAL::FactorType FactorType; + typedef typename FactorGraph::Eliminate Eliminate; + + sharedConditional conditional_; + weak_ptr parent_; + std::list children_; + + friend class BayesTree; + + /** Default constructor */ + BayesTreeClique() {} + + /** Construct from a conditional, leaving parent and child pointers uninitialized */ + BayesTreeClique(const sharedConditional& conditional); + + virtual ~BayesTreeClique() {} + + /** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */ + static shared_ptr Create(const sharedConditional& conditional) { return shared_ptr(new BayesTreeClique(conditional)); } + + /** Construct shared_ptr from a FactorGraph::EliminationResult. In this class + * the conditional part is kept and the factor part is ignored, but in derived clique + * types, such as ISAM2Clique, the factor part is kept as a cached factor. + * @param + */ + template + static shared_ptr Create(const RESULT& result) { return Create(result.first); } + + /** print this node */ + void print(const std::string& s = "") const; + + /** The arrow operator accesses the conditional */ + const CONDITIONAL* operator->() const { return conditional_.get(); } + + /** The arrow operator accesses the conditional */ + CONDITIONAL* operator->() { return conditional_.get(); } + + /** Access the conditional */ + const sharedConditional& conditional() const { return conditional_; } + + /** is this the root of a Bayes tree ? */ + inline bool isRoot() const { return parent_.expired(); } + + /** return the const reference of children */ + std::list& children() { return children_; } + const std::list& children() const { return children_; } + + /** The size of subtree rooted at this clique, i.e., nr of Cliques */ + size_t treeSize() const; + + /** print this node and entire subtree below it */ + void printTree(const std::string& indent="") const; + + /** Permute the variables in the whole subtree rooted at this clique */ + void permuteWithInverse(const Permutation& inversePermutation); + + /** Permute variables when they only appear in the separators. In this + * case the running intersection property will be used to prevent always + * traversing the whole tree. Returns whether any separator variables in + * this subtree were reordered. + */ + bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + + /** return the conditional P(S|Root) on the separator given the root */ + // TODO: create a cached version + BayesNet shortcut(shared_ptr root, Eliminate function); + + /** return the marginal P(C) of the clique */ + FactorGraph marginal(shared_ptr root, Eliminate function); + + /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ + FactorGraph joint(shared_ptr C2, shared_ptr root, Eliminate function); + + bool equals(const This& other, double tol=1e-9) const { + return (!conditional_ && !other.conditional()) || + conditional_->equals(*(other.conditional()), tol); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(conditional_); + ar & BOOST_SERIALIZATION_NVP(parent_); + ar & BOOST_SERIALIZATION_NVP(children_); + } + + }; // \struct Clique + + + } /// namespace gtsam diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index 533f36c05..e776a4390 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -126,28 +126,29 @@ namespace gtsam { } /* ************************************************************************* */ - template + template void _FactorGraph_BayesTree_adder( vector::sharedFactor>& factors, - const typename BayesTree::sharedClique& clique) { + const typename BayesTree::sharedClique& clique) { if(clique) { // Add factor from this clique factors.push_back((*clique)->toFactor()); // Traverse children - BOOST_FOREACH(const typename BayesTree::sharedClique& child, clique->children()) { - _FactorGraph_BayesTree_adder(factors, child); + typedef typename BayesTree::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& child, clique->children()) { + _FactorGraph_BayesTree_adder(factors, child); } } } /* ************************************************************************* */ template - template - FactorGraph::FactorGraph(const BayesTree& bayesTree) { + template + FactorGraph::FactorGraph(const BayesTree& bayesTree) { factors_.reserve(bayesTree.size()); - _FactorGraph_BayesTree_adder(factors_, bayesTree.root()); + _FactorGraph_BayesTree_adder(factors_, bayesTree.root()); } /* ************************************************************************* */ diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 91a879392..645b496ec 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -33,7 +33,7 @@ namespace gtsam { // Forward declarations -template class BayesTree; +template class BayesTree; /** * A factor graph is a bipartite graph with factor nodes connected to variable nodes. @@ -78,8 +78,8 @@ template class BayesTree; FactorGraph(const BayesNet& bayesNet); /** convert from Bayes net */ - template - FactorGraph(const BayesTree& bayesTree); + template + FactorGraph(const BayesTree& bayesTree); /** convert from a derived type */ template diff --git a/gtsam/inference/GenericMultifrontalSolver-inl.h b/gtsam/inference/GenericMultifrontalSolver-inl.h index 30685a751..d2c384c0c 100644 --- a/gtsam/inference/GenericMultifrontalSolver-inl.h +++ b/gtsam/inference/GenericMultifrontalSolver-inl.h @@ -51,14 +51,14 @@ namespace gtsam { /* ************************************************************************* */ template - typename JT::BayesTree::shared_ptr GenericMultifrontalSolver::eliminate( + typename BayesTree::shared_ptr GenericMultifrontalSolver::eliminate( typename FactorGraph::Eliminate function) const { // eliminate junction tree, returns pointer to root - typename JT::BayesTree::sharedClique root = junctionTree_->eliminate(function); + typename BayesTree::sharedClique root = junctionTree_->eliminate(function); // create an empty Bayes tree and insert root clique - typename JT::BayesTree::shared_ptr bayesTree(new typename JT::BayesTree); + typename BayesTree::shared_ptr bayesTree(new BayesTree); bayesTree->insert(root); // return the Bayes tree diff --git a/gtsam/inference/GenericMultifrontalSolver.h b/gtsam/inference/GenericMultifrontalSolver.h index 4228a058b..2375ff08c 100644 --- a/gtsam/inference/GenericMultifrontalSolver.h +++ b/gtsam/inference/GenericMultifrontalSolver.h @@ -79,7 +79,7 @@ namespace gtsam { * Eliminate the factor graph sequentially. Uses a column elimination tree * to recursively eliminate. */ - typename JUNCTIONTREE::BayesTree::shared_ptr + typename BayesTree::shared_ptr eliminate(Eliminate function) const; /** diff --git a/gtsam/inference/JunctionTree-inl.h b/gtsam/inference/JunctionTree-inl.h index 6ca112c18..b3f5edd49 100644 --- a/gtsam/inference/JunctionTree-inl.h +++ b/gtsam/inference/JunctionTree-inl.h @@ -36,8 +36,8 @@ namespace gtsam { using namespace std; /* ************************************************************************* */ - template - void JunctionTree::construct(const FG& fg, const VariableIndex& variableIndex) { + template + void JunctionTree::construct(const FG& fg, const VariableIndex& variableIndex) { tic(1, "JT Constructor"); tic(1, "JT symbolic ET"); const typename EliminationTree::shared_ptr symETree = @@ -58,8 +58,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - JunctionTree::JunctionTree(const FG& fg) { + template + JunctionTree::JunctionTree(const FG& fg) { tic(0, "VariableIndex"); VariableIndex varIndex(fg); toc(0, "VariableIndex"); @@ -67,14 +67,14 @@ namespace gtsam { } /* ************************************************************************* */ - template - JunctionTree::JunctionTree(const FG& fg, const VariableIndex& variableIndex) { + template + JunctionTree::JunctionTree(const FG& fg, const VariableIndex& variableIndex) { construct(fg, variableIndex); } /* ************************************************************************* */ - template - typename JunctionTree::sharedClique JunctionTree::distributeFactors( + template + typename JunctionTree::sharedClique JunctionTree::distributeFactors( const FG& fg, const typename SymbolicBayesTree::sharedClique& bayesClique) { // Build "target" index. This is an index for each variable of the factors @@ -109,8 +109,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename JunctionTree::sharedClique JunctionTree::distributeFactors(const FG& fg, + template + typename JunctionTree::sharedClique JunctionTree::distributeFactors(const FG& fg, const std::vector >& targets, const SymbolicBayesTree::sharedClique& bayesClique) { @@ -147,21 +147,21 @@ namespace gtsam { } /* ************************************************************************* */ - template - pair::BayesTree::sharedClique, - typename FG::sharedFactor> JunctionTree::eliminateOneClique( + template + pair::BTClique::shared_ptr, + typename FG::sharedFactor> JunctionTree::eliminateOneClique( typename FG::Eliminate function, - const boost::shared_ptr& current, bool cache) const { + const boost::shared_ptr& current) const { FG fg; // factor graph will be assembled from local factors and marginalized children fg.reserve(current->size() + current->children().size()); fg.push_back(*current); // add the local factors // receive the factors from the child and its clique point - list children; + list children; BOOST_FOREACH(const boost::shared_ptr& child, current->children()) { - pair tree_factor( - eliminateOneClique(function, child, cache)); + pair tree_factor( + eliminateOneClique(function, child)); children.push_back(tree_factor.first); fg.push_back(tree_factor.second); } @@ -172,9 +172,7 @@ namespace gtsam { // Now that we know which factors and variables, and where variables // come from and go to, create and eliminate the new joint factor. tic(2, "CombineAndEliminate"); - pair< - typename FG::FactorType::ConditionalType::shared_ptr, - typename FG::sharedFactor> eliminated(function(fg, + typename FG::EliminationResult eliminated(function(fg, current->frontal.size())); toc(2, "CombineAndEliminate"); @@ -182,33 +180,31 @@ namespace gtsam { tic(3, "Update tree"); // create a new clique corresponding the combined factors - typename BayesTree::sharedClique new_clique(new typename BayesTree::Clique(eliminated.first)); + typename BTClique::shared_ptr new_clique(BTClique::Create(eliminated)); new_clique->children_ = children; - BOOST_FOREACH(typename BayesTree::sharedClique& childRoot, children) { + BOOST_FOREACH(typename BTClique::shared_ptr& childRoot, children) { childRoot->parent_ = new_clique; } - if(cache) - new_clique->cachedFactor() = eliminated.second; toc(3, "Update tree"); return make_pair(new_clique, eliminated.second); } /* ************************************************************************* */ - template - typename JunctionTree::BayesTree::sharedClique JunctionTree::eliminate( - typename FG::Eliminate function, bool cache) const { + template + typename BTCLIQUE::shared_ptr JunctionTree::eliminate( + typename FG::Eliminate function) const { if (this->root()) { tic(2, "JT eliminate"); - pair ret = - this->eliminateOneClique(function, this->root(), cache); + pair ret = + this->eliminateOneClique(function, this->root()); if (ret.second->size() != 0) throw runtime_error( "JuntionTree::eliminate: elimination failed because of factors left over!"); toc(2, "JT eliminate"); return ret.first; } else - return typename BayesTree::sharedClique(); + return typename BTClique::shared_ptr(); } } //namespace gtsam diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h index 909cebc62..12c7dd5d2 100644 --- a/gtsam/inference/JunctionTree.h +++ b/gtsam/inference/JunctionTree.h @@ -45,7 +45,7 @@ namespace gtsam { * * \ingroup Multifrontal */ - template + template::Clique> class JunctionTree: public ClusterTree { public: @@ -55,7 +55,7 @@ namespace gtsam { typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique /// The BayesTree type produced by elimination - typedef class BayesTree BayesTree; + typedef BTCLIQUE BTClique; /// Shared pointer to this class typedef boost::shared_ptr > shared_ptr; @@ -73,9 +73,9 @@ namespace gtsam { const SymbolicBayesTree::sharedClique& clique); // recursive elimination function - std::pair + std::pair eliminateOneClique(typename FG::Eliminate function, - const boost::shared_ptr& clique, bool cache = false) const; + const boost::shared_ptr& clique) const; // internal constructor void construct(const FG& fg, const VariableIndex& variableIndex); @@ -103,12 +103,9 @@ namespace gtsam { /** Eliminate the factors in the subgraphs to produce a BayesTree. * @param function The function used to eliminate, see the namespace functions * in GaussianFactorGraph.h - * @param cache Whether to cache the intermediate elimination factors for use in ISAM2 - this - * should always be false when called outside of ISAM2 (this will be fixed in the future). * @return The BayesTree resulting from elimination */ - typename BayesTree::sharedClique eliminate(typename FG::Eliminate function, - bool cache = false) const; + typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const; }; // JunctionTree diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 46462f60f..3ace299cb 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -42,9 +42,6 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : Base(CBN) {} - /* ************************************************************************* */ - GaussianFactorGraph::GaussianFactorGraph(const BayesTree& GBT) : Base(GBT) {} - /* ************************************************************************* */ GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { FastSet keys; diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 6b937c214..3ec8a78f0 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -75,7 +75,8 @@ namespace gtsam { /** * Constructor that receives a BayesTree and returns a GaussianFactorGraph */ - GaussianFactorGraph(const BayesTree& GBT); + template + GaussianFactorGraph(const BayesTree& gbt) : Base(gbt) {} /** Constructor from a factor graph of GaussianFactor or a derived type */ template diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp index f539e1759..ff66dffd0 100644 --- a/gtsam/linear/GaussianJunctionTree.cpp +++ b/gtsam/linear/GaussianJunctionTree.cpp @@ -34,7 +34,7 @@ namespace gtsam { using namespace std; /* ************************************************************************* */ - void GaussianJunctionTree::btreeBackSubstitute(const boost::shared_ptr& current, VectorValues& config) const { + void GaussianJunctionTree::btreeBackSubstitute(const BTClique::shared_ptr& current, VectorValues& config) const { // solve the bayes net in the current node current->conditional()->solveInPlace(config); @@ -47,15 +47,15 @@ namespace gtsam { // } // solve the bayes nets in the child nodes - BOOST_FOREACH(const BayesTree::sharedClique& child, current->children()) { + BOOST_FOREACH(const BTClique::shared_ptr& child, current->children()) { btreeBackSubstitute(child, config); } } /* ************************************************************************* */ - void GaussianJunctionTree::btreeRHS(const boost::shared_ptr& current, VectorValues& config) const { + void GaussianJunctionTree::btreeRHS(const BTClique::shared_ptr& current, VectorValues& config) const { current->conditional()->rhs(config); - BOOST_FOREACH(const BayesTree::sharedClique& child, current->children()) + BOOST_FOREACH(const BTClique::shared_ptr& child, current->children()) btreeRHS(child, config); } @@ -63,7 +63,7 @@ namespace gtsam { VectorValues GaussianJunctionTree::optimize(Eliminate function) const { tic(1, "GJT eliminate"); // eliminate from leaves to the root - boost::shared_ptr rootClique(this->eliminate(function)); + BTClique::shared_ptr rootClique(this->eliminate(function)); toc(1, "GJT eliminate"); // Allocate solution vector and copy RHS diff --git a/gtsam/linear/GaussianJunctionTree.h b/gtsam/linear/GaussianJunctionTree.h index 0397a646b..678c7c616 100644 --- a/gtsam/linear/GaussianJunctionTree.h +++ b/gtsam/linear/GaussianJunctionTree.h @@ -43,10 +43,10 @@ namespace gtsam { protected: // back-substitute in topological sort order (parents first) - void btreeBackSubstitute(const boost::shared_ptr& current, VectorValues& config) const; + void btreeBackSubstitute(const BTClique::shared_ptr& current, VectorValues& config) const; // find the RHS for the system in order to perform backsubstitution - void btreeRHS(const boost::shared_ptr& current, VectorValues& config) const; + void btreeRHS(const BTClique::shared_ptr& current, VectorValues& config) const; public : @@ -64,22 +64,22 @@ namespace gtsam { VectorValues optimize(Eliminate function) const; // convenient function to return dimensions of all variables in the BayesTree - template - static void countDims(const BayesTree& bayesTree, DIM_CONTAINER& dims) { + template + static void countDims(const BayesTree& bayesTree, DIM_CONTAINER& dims) { dims = DIM_CONTAINER(bayesTree.root()->conditional()->back()+1, 0); countDims(bayesTree.root(), dims); } private: - template - static void countDims(const boost::shared_ptr& clique, DIM_CONTAINER& dims) { + template + static void countDims(const boost::shared_ptr& clique, DIM_CONTAINER& dims) { GaussianConditional::const_iterator it = clique->conditional()->beginFrontals(); for (; it != clique->conditional()->endFrontals(); ++it) { assert(dims.at(*it) == 0); dims.at(*it) = clique->conditional()->dim(it); } - BOOST_FOREACH(const boost::shared_ptr& child, clique->children()) { + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children()) { countDims(child, dims); } } diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index d32085eeb..95a7ca346 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -85,7 +85,7 @@ struct ISAM2::Impl { * * Alternatively could we trace up towards the root for each variable here? */ - static void FindAll(ISAM2Type::sharedClique clique, FastSet& keys, const vector& markedMask); + static void FindAll(typename BayesTreeClique::shared_ptr clique, FastSet& keys, const vector& markedMask); /** * Apply expmap to the given values, but only for indices appearing in @@ -191,8 +191,8 @@ FastSet ISAM2::Impl::CheckRelinearization(Permuted -void ISAM2::Impl::FindAll(ISAM2Type::sharedClique clique, FastSet& keys, const vector& markedMask) { +template +void ISAM2::Impl::FindAll(typename BayesTreeClique::shared_ptr clique, FastSet& keys, const vector& markedMask) { static const bool debug = false; // does the separator contain any of the variables? bool found = false; @@ -206,7 +206,7 @@ void ISAM2::Impl::FindAll(ISAM2Type::sharedClique clique, F if(debug) clique->print("Key(s) marked in clique "); if(debug) cout << "so marking key " << (*clique)->keys().front() << endl; } - BOOST_FOREACH(const sharedClique& child, clique->children_) { + BOOST_FOREACH(const typename BayesTreeClique::shared_ptr& child, clique->children_) { FindAll(child, keys, markedMask); } } @@ -338,8 +338,8 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, // eliminate into a Bayes net tic(7,"eliminate"); - GaussianJunctionTree jt(factors, affectedFactorsIndex); - result.bayesTree = jt.eliminate(EliminatePreferLDL, true); + JunctionTree jt(factors, affectedFactorsIndex); + result.bayesTree = jt.eliminate(EliminatePreferLDL); if(debug && result.bayesTree) { cout << "Re-eliminated BT:\n"; result.bayesTree->printTree(""); diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 383951e6d..461964359 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -192,7 +192,11 @@ boost::shared_ptr > ISAM2::recalculate( sharedClique clique = this->nodes_[key]; while(clique) { affectedStructuralKeys.insert((*clique)->beginFrontals(), (*clique)->endFrontals()); - clique = clique->parent_.lock(); +#ifndef NDEBUG // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique + clique = boost::dynamic_pointer_cast(clique->parent_.lock()); +#else + clique = boost::static_pointer_cast(clique->parent_.lock()); +#endif } } toc(0, "affectedStructuralKeys"); @@ -286,13 +290,13 @@ boost::shared_ptr > ISAM2::recalculate( toc(2,"linearize"); tic(5,"eliminate"); - GaussianJunctionTree jt(factors, variableIndex_); - sharedClique newRoot = jt.eliminate(EliminatePreferLDL, true); + JunctionTree jt(factors, variableIndex_); + sharedClique newRoot = jt.eliminate(EliminatePreferLDL); if(debug) newRoot->print("Eliminated: "); toc(5,"eliminate"); tic(6,"insert"); - BayesTree::clear(); + Base::clear(); this->insert(newRoot); toc(6,"insert"); @@ -550,6 +554,8 @@ ISAM2Result ISAM2::update( result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); toc(10,"evaluate error after"); + result.cliques = this->nodes().size(); + return result; } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 8159ebfb5..e76a33105 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -113,6 +113,87 @@ struct ISAM2Result { * will be reeliminated. */ size_t variablesReeliminated; + + /** The number of cliques in the Bayes' Tree */ + size_t cliques; +}; + +template +struct ISAM2Clique : public BayesTreeClique { + + typedef ISAM2Clique This; + typedef BayesTreeClique Base; + + typedef boost::shared_ptr shared_ptr; + + typename Base::FactorType::shared_ptr cachedFactor_; + + /** Access the cached factor */ + typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } + + /** Construct from an elimination result, caches the eliminated factor */ + template + ISAM2Clique(const RESULT& result) : Base(result), cachedFactor_(result.second) {} + + /** Construct from a conditional */ + ISAM2Clique(const typename Base::sharedConditional& conditional) : Base(conditional) {} + + /** Create from an elimination result, overrides BayesTreeClique::Create(const RESULT&) to cache the eliminated factor */ + template + static shared_ptr Create(const RESULT& result) { return shared_ptr(new This(result)); } + + static shared_ptr Create(const typename Base::sharedConditional& conditional) { return shared_ptr(new This(conditional)); } + + void permuteWithInverse(const Permutation& inversePermutation) { + Base::conditional_->permuteWithInverse(inversePermutation); + if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); + BOOST_FOREACH(const typename Base::shared_ptr& child, Base::children_) { + shared_ptr _child; +#ifndef NDEBUG // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique + _child = boost::dynamic_pointer_cast(child); +#else + _child = boost::static_pointer_cast(child); +#endif + _child->permuteWithInverse(inversePermutation); + } + Base::assertInvariants(); + } + + bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { + bool changed = Base::conditional_->permuteSeparatorWithInverse(inversePermutation); +#ifndef NDEBUG + if(!changed) { + BOOST_FOREACH(Index& separatorKey, Base::conditional_->parents()) { + assert(separatorKey == inversePermutation[separatorKey]); } + BOOST_FOREACH(const typename Base::shared_ptr& child, Base::children_) { + shared_ptr _child = boost::dynamic_pointer_cast(child); // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique + assert(_child->permuteSeparatorWithInverse(inversePermutation) == false); } + } +#endif + if(changed) { + if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); + BOOST_FOREACH(const typename Base::shared_ptr& child, Base::children_) { + shared_ptr _child; + #ifndef NDEBUG // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique + _child = boost::dynamic_pointer_cast(child); + #else + _child = boost::static_pointer_cast(child); + #endif + (void)_child->permuteSeparatorWithInverse(inversePermutation); + } + } + Base::assertInvariants(); + return changed; + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(cachedFactor_); + } }; /** @@ -125,7 +206,7 @@ struct ISAM2Result { * estimate of all variables. */ template -class ISAM2: public BayesTree { +class ISAM2: public BayesTree > { protected: @@ -168,7 +249,7 @@ private: public: - typedef BayesTree Base; ///< The BayesTree base class + typedef BayesTree > Base; ///< The BayesTree base class typedef ISAM2 This; ///< This class /** Create an empty ISAM2 instance */ @@ -177,8 +258,9 @@ public: /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ ISAM2(); - typedef typename BayesTree::sharedClique sharedClique; ///< Shared pointer to a clique - typedef typename BayesTree::Cliques Cliques; ///< List of Clique typedef from base class + typedef typename Base::Clique Clique; ///< A clique + typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique + typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class /** * Add new factors, updating the solution and relinearizing as needed. diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index ad9855fa5..378afade3 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -167,7 +167,7 @@ TEST(ISAM2, optimize2) { conditional->solveInPlace(expected); // Clique - GaussianISAM2::sharedClique clique(new GaussianISAM2::Clique(conditional)); + GaussianISAM2::sharedClique clique(GaussianISAM2::Clique::Create(conditional)); VectorValues actual(theta.dims(ordering)); conditional->rhs(actual); optimize2(clique, actual); From 7a95ccbd8675e82f02852c17fd7a67d91a0c7f89 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 29 Nov 2011 16:49:12 +0000 Subject: [PATCH 03/17] (in branch) In progress refactoring to store gradient for dogleg --- gtsam/inference/BayesTree-inl.h | 2 +- gtsam/inference/BayesTree.h | 41 +++++++++++++++++++++++---------- 2 files changed, 30 insertions(+), 13 deletions(-) diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index c63553ac2..cddd642f0 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -730,7 +730,7 @@ namespace gtsam { this->removeClique(clique); // remove path above me - this->removePath(sharedClique(clique->parent_.lock()), bn, orphans); + this->removePath(typename BayesTreeClique::shared_ptr(clique->parent_.lock()), bn, orphans); // add children to list of orphans (splice also removed them from clique->children_) orphans.splice (orphans.begin(), clique->children_); diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index eeebf45bc..45dea6e49 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -304,35 +304,52 @@ namespace gtsam { * * Since our Conditional class already handles multiple frontal variables, * this Clique contains exactly 1 conditional. + * + * This is the default clique type in a BayesTree, but some algorithms, like + * iSAM2 (see ISAM2Clique), use a different clique type in order to store + * extra data along with the clique. */ - template + template struct BayesTreeClique { + }; + + + /* ************************************************************************* */ + /** + * This is the base class for BayesTree cliques. The default and standard + * derived type is BayesTreeClique, but some algorithms, like iSAM2, use a + * different clique type in order to store extra data along with the clique. + */ + template + struct BayesTreeCliqueBase { + protected: void assertInvariants() const; public: - typedef BayesTreeClique This; - typedef CONDITIONAL ConditionalType; - typedef boost::shared_ptr sharedConditional; + typedef BayesTreeClique This; + typedef DERIVED DerivedType; + typedef typename DERIVED::ConditionalType ConditionalType; + typedef boost::shared_ptr sharedConditional; typedef typename boost::shared_ptr shared_ptr; typedef typename boost::weak_ptr weak_ptr; - typedef typename CONDITIONAL::FactorType FactorType; + typedef typename ConditionalType::FactorType FactorType; typedef typename FactorGraph::Eliminate Eliminate; sharedConditional conditional_; weak_ptr parent_; std::list children_; - friend class BayesTree; + friend class BayesTree; /** Default constructor */ - BayesTreeClique() {} + BayesTreeCliqueBase() {} /** Construct from a conditional, leaving parent and child pointers uninitialized */ - BayesTreeClique(const sharedConditional& conditional); + BayesTreeCliqueBase(const sharedConditional& conditional); - virtual ~BayesTreeClique() {} + virtual ~BayesTreeCliqueBase() {} /** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */ static shared_ptr Create(const sharedConditional& conditional) { return shared_ptr(new BayesTreeClique(conditional)); } @@ -349,10 +366,10 @@ namespace gtsam { void print(const std::string& s = "") const; /** The arrow operator accesses the conditional */ - const CONDITIONAL* operator->() const { return conditional_.get(); } + const ConditionalType* operator->() const { return conditional_.get(); } /** The arrow operator accesses the conditional */ - CONDITIONAL* operator->() { return conditional_.get(); } + ConditionalType* operator->() { return conditional_.get(); } /** Access the conditional */ const sharedConditional& conditional() const { return conditional_; } @@ -382,7 +399,7 @@ namespace gtsam { /** return the conditional P(S|Root) on the separator given the root */ // TODO: create a cached version - BayesNet shortcut(shared_ptr root, Eliminate function); + BayesNet shortcut(shared_ptr root, Eliminate function); /** return the marginal P(C) of the clique */ FactorGraph marginal(shared_ptr root, Eliminate function); From a0abe68b6438881b804f7c0a5f5d6dac9ffe7968 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 29 Nov 2011 17:02:02 +0000 Subject: [PATCH 04/17] (in branch) Merged from trunk r7760-r7959 --- examples/PlanarSLAMSelfContained_advanced.cpp | 2 +- examples/SimpleRotation.cpp | 2 +- examples/easyPoint2KalmanFilter.cpp | 2 +- examples/elaboratePoint2KalmanFilter.cpp | 2 +- examples/vSLAMexample/vISAMexample.cpp | 4 +- gtsam.h | 6 + gtsam/base/Group.h | 5 +- gtsam/base/Lie.h | 202 +++++++++--------- gtsam/base/Manifold.h | 106 +++++---- gtsam/base/cholesky.cpp | 4 +- gtsam/base/cholesky.h | 2 +- gtsam/geometry/Point2.h | 94 ++++---- gtsam/geometry/Point3.h | 93 ++++---- gtsam/geometry/Pose2.h | 6 +- gtsam/geometry/Pose3.cpp | 4 +- gtsam/geometry/Pose3.h | 85 ++++---- gtsam/geometry/Rot2.h | 99 +++++---- gtsam/geometry/Rot3M.h | 118 +++++----- gtsam/geometry/Rot3Q.h | 118 +++++----- gtsam/geometry/StereoPoint2.h | 84 ++++---- gtsam/nonlinear/DoglegOptimizerImpl.h | 5 +- gtsam/nonlinear/GaussianISAM2.cpp | 1 - gtsam/nonlinear/GaussianISAM2.h | 14 +- gtsam/nonlinear/ISAM2-impl-inl.h | 44 ++-- gtsam/nonlinear/ISAM2-inl.h | 83 ++++--- gtsam/nonlinear/ISAM2.h | 38 ++-- gtsam/nonlinear/NonlinearFactorGraph.h | 2 +- gtsam/nonlinear/NonlinearISAM-inl.h | 21 +- gtsam/nonlinear/NonlinearISAM.h | 8 +- gtsam/nonlinear/NonlinearOptimizer-inl.h | 6 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 8 +- gtsam/nonlinear/TupleValues-inl.h | 23 -- gtsam/nonlinear/TupleValues.h | 25 ++- gtsam/nonlinear/Values-inl.h | 4 - gtsam/nonlinear/Values.h | 4 +- gtsam/nonlinear/tests/testValues.cpp | 2 +- gtsam/slam/PartialPriorFactor.h | 56 ++--- gtsam/slam/PlanarSLAMOdometry.h | 2 +- gtsam/slam/ProjectionFactor.h | 5 + gtsam/slam/Simulated3D.cpp | 8 - gtsam/slam/planarSLAM.cpp | 3 - gtsam/slam/pose2SLAM.cpp | 2 - gtsam/slam/pose3SLAM.cpp | 2 - gtsam/slam/simulated2D.cpp | 8 - gtsam/slam/simulated2DOriented.cpp | 4 - gtsam/slam/smallExample.cpp | 1 - gtsam/slam/tests/testGeneralSFMFactor.cpp | 2 +- .../testGeneralSFMFactor_Cal3Bundler.cpp | 2 +- gtsam/slam/tests/testPose3SLAM.cpp | 5 +- gtsam/slam/visualSLAM.cpp | 3 +- myconfigure.ardrone | 1 + tests/testExtendedKalmanFilter.cpp | 2 +- tests/testGraph.cpp | 1 - tests/testNonlinearEquality.cpp | 2 - tests/testNonlinearFactor.cpp | 1 - tests/testNonlinearISAM.cpp | 2 +- tests/testRot3QOptimization.cpp | 2 +- tests/testTupleValues.cpp | 2 +- 58 files changed, 741 insertions(+), 701 deletions(-) create mode 100755 myconfigure.ardrone diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index 549acabae..c8ee21997 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -34,7 +34,7 @@ #include // implementations for structures - needed if self-contained, and these should be included last -#include +#include #include #include #include diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index c31e91b5f..438459e64 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index 3b8ee2980..f22d65f95 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include using namespace std; diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 73efc653c..8c8cbe147 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include #include diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index 300124c4e..ed3a68ba8 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -27,9 +27,7 @@ using namespace boost; #include #include #include -#include -#include -#include +#include #include "vSLAMutils.h" #include "Feature2D.h" diff --git a/gtsam.h b/gtsam.h index 52422cf3c..331bdd09a 100644 --- a/gtsam.h +++ b/gtsam.h @@ -178,3 +178,9 @@ class PlanarSLAMOdometry { void print(string s) const; GaussianFactor* linearize(const PlanarSLAMValues& center, const Ordering& ordering) const; }; + +class GaussianSequentialSolver { + GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); + GaussianBayesNet* eliminate(); + VectorValues* optimize(); +}; diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 3c56a541a..533c042c0 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -2,8 +2,6 @@ * @file Group.h * * @brief Concept check class for variable types with Group properties - * A Group concept extends a Manifold - * * @date Nov 5, 2011 * @author Alex Cunningham */ @@ -13,7 +11,8 @@ namespace gtsam { /** - * Concept check for general Group structure + * This concept check enforces a Group structure on a variable type, + * in which we require the existence of basic algebraic operations. */ template class GroupConcept { diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 756790f16..932e069ef 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -14,28 +14,6 @@ * @brief Base class and basic functions for Lie types * @author Richard Roberts * @author Alex Cunningham - * - * This concept check provides a specialization on the Manifold type, - * in which the Manifolds represented require an algebra and group structure. - * All Lie types must also be a Manifold. - * - * The necessary functions to implement for Lie are defined - * below with additional details as to the interface. The - * concept checking function in class Lie will check whether or not - * the function exists and throw compile-time errors. - * - * Expmap around identity - * static T Expmap(const Vector& v); - * - * - * Logmap around identity - * static Vector Logmap(const T& p); - * - * Compute l0 s.t. l2=l1*l0, where (*this) is l1 - * A default implementation of between(*this, lp) is available: - * between_default() - * T between(const T& l2) const; - * */ @@ -46,89 +24,115 @@ namespace gtsam { - /** - * These core global functions can be specialized by new Lie types - * for better performance. - */ +/** + * These core global functions can be specialized by new Lie types + * for better performance. + */ - /** Compute l0 s.t. l2=l1*l0 */ - template - inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);} +/** Compute l0 s.t. l2=l1*l0 */ +template +inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);} - /** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */ - template - inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));} +/** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */ +template +inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));} - /** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ - template - inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));} +/** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ +template +inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));} - /** - * Concept check class for Lie group type - * - * T is the Lie type, like Point2, Pose3, etc. - * - * By convention, we use capital letters to designate a static function - */ - template - class LieConcept { - private: - /** concept checking function - implement the functions this demands */ - static void concept_check(const T& t) { +/** + * Concept check class for Lie group type + * + * This concept check provides a specialization on the Manifold type, + * in which the Manifolds represented require an algebra and group structure. + * All Lie types must also be a Manifold. + * + * The necessary functions to implement for Lie are defined + * below with additional details as to the interface. The + * concept checking function in class Lie will check whether or not + * the function exists and throw compile-time errors. + * + * The exponential map is a specific retraction for Lie groups, + * which maps the tangent space in canonical coordinates back to + * the underlying manifold. Because we can enforce a group structure, + * a retraction of delta v, with tangent space centered at x1 can be performed + * as x2 = x1.compose(Expmap(v)). + * + * Expmap around identity + * static T Expmap(const Vector& v); + * + * Logmap around identity + * static Vector Logmap(const T& p); + * + * Compute l0 s.t. l2=l1*l0, where (*this) is l1 + * A default implementation of between(*this, lp) is available: + * between_default() + * T between(const T& l2) const; + * + * By convention, we use capital letters to designate a static function + * + * @tparam T is a Lie type, like Point2, Pose3, etc. + */ +template +class LieConcept { +private: + /** concept checking function - implement the functions this demands */ + static void concept_check(const T& t) { - /** assignment */ - T t2 = t; + /** assignment */ + T t2 = t; - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); + /** + * Returns dimensionality of the tangent space + */ + size_t dim_ret = t.dim(); - /** expmap around identity */ - T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); + /** expmap around identity */ + T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); - /** Logmap around identity */ - Vector logmap_identity_ret = T::Logmap(t); + /** Logmap around identity */ + Vector logmap_identity_ret = T::Logmap(t); - /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ - T between_ret = t.between(t2); - } - - }; - - /** - * Three term approximation of the Baker�Campbell�Hausdorff formula - * In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) - * it is not true that Z = X+Y. Instead, Z can be calculated using the BCH - * formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 - * http://en.wikipedia.org/wiki/Baker�Campbell�Hausdorff_formula - */ - /// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? - template - T BCH(const T& X, const T& Y) { - static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; - T X_Y = bracket(X, Y); - return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, - bracket(X, X_Y)); + /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ + T between_ret = t.between(t2); } - /** - * Declaration of wedge (see Murray94book) used to convert - * from n exponential coordinates to n*n element of the Lie algebra - */ - template Matrix wedge(const Vector& x); +}; - /** - * Exponential map given exponential coordinates - * class T needs a wedge<> function and a constructor from Matrix - * @param x exponential coordinates, vector of size n - * @ return a T - */ - template - T expm(const Vector& x, int K=7) { - Matrix xhat = wedge(x); - return expm(xhat,K); - } +/** + * Three term approximation of the Baker�Campbell�Hausdorff formula + * In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) + * it is not true that Z = X+Y. Instead, Z can be calculated using the BCH + * formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 + * http://en.wikipedia.org/wiki/Baker�Campbell�Hausdorff_formula + */ +/// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? +template +T BCH(const T& X, const T& Y) { + static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; + T X_Y = bracket(X, Y); + return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, + bracket(X, X_Y)); +} + +/** + * Declaration of wedge (see Murray94book) used to convert + * from n exponential coordinates to n*n element of the Lie algebra + */ +template Matrix wedge(const Vector& x); + +/** + * Exponential map given exponential coordinates + * class T needs a wedge<> function and a constructor from Matrix + * @param x exponential coordinates, vector of size n + * @ return a T + */ +template +T expm(const Vector& x, int K=7) { + Matrix xhat = wedge(x); + return expm(xhat,K); +} } // namespace gtsam @@ -141,11 +145,11 @@ namespace gtsam { * the gtsam namespace to be more easily enforced as testable */ #define GTSAM_CONCEPT_LIE_INST(T) \ - template class gtsam::ManifoldConcept; \ - template class gtsam::GroupConcept; \ - template class gtsam::LieConcept; + template class gtsam::ManifoldConcept; \ + template class gtsam::GroupConcept; \ + template class gtsam::LieConcept; #define GTSAM_CONCEPT_LIE_TYPE(T) \ - typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ - typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ - typedef gtsam::LieConcept _gtsam_LieConcept_##T; + typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ + typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ + typedef gtsam::LieConcept _gtsam_LieConcept_##T; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 87506f31f..430be4f7d 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -12,23 +12,7 @@ /** * @file Manifold.h * @brief Base class and basic functions for Manifold types - * @author Richard Roberts * @author Alex Cunningham - * - * The necessary functions to implement for Manifold are defined - * below with additional details as to the interface. The - * concept checking function in class Manifold will check whether or not - * the function exists and throw compile-time errors. - * - * Returns dimensionality of the tangent space - * inline size_t dim() const; - * - * Returns Retraction update of T - * T retract(const Vector& v) const; - * - * Returns inverse retraction operation - * Vector localCoordinates(const T& lp) const; - * */ #pragma once @@ -38,40 +22,70 @@ namespace gtsam { - /** - * Concept check class for Manifold types - * Requires a mapping between a linear tangent space and the underlying - * manifold, of which Lie is a specialization. - * - * T is the Manifold type, like Point2, Pose3, etc. - * - * By convention, we use capital letters to designate a static function - */ - template - class ManifoldConcept { - private: - /** concept checking function - implement the functions this demands */ - static void concept_check(const T& t) { +/** + * Concept check class for Manifold types + * Requires a mapping between a linear tangent space and the underlying + * manifold, of which Lie is a specialization. + * + * The necessary functions to implement for Manifold are defined + * below with additional details as to the interface. The + * concept checking function in class Manifold will check whether or not + * the function exists and throw compile-time errors. + * + * A manifold defines a space in which there is a notion of a linear tangent space + * that can be centered around a given point on the manifold. These nonlinear + * spaces may have such properties as wrapping around (as is the case with rotations), + * which might make linear operations on parameters not return a viable element of + * the manifold. + * + * We perform optimization by computing a linear delta in the tangent space of the + * current estimate, and then apply this change using a retraction operation, which + * maps the change in tangent space back to the manifold itself. + * + * There may be multiple possible retractions for a given manifold, which can be chosen + * between depending on the computational complexity. The important criteria for + * the creation for the retract and localCoordinates functions is that they be + * inverse operations. + * + * Returns dimensionality of the tangent space, which may be smaller than the number + * of nonlinear parameters. + * size_t dim() const; + * + * Returns a new T that is a result of updating *this with the delta v after pulling + * the updated value back to the manifold T. + * T retract(const Vector& v) const; + * + * Returns the linear coordinates of lp in the tangent space centered around *this. + * Vector localCoordinates(const T& lp) const; + * + * By convention, we use capital letters to designate a static function + * @tparam T is a Lie type, like Point2, Pose3, etc. + */ +template +class ManifoldConcept { +private: + /** concept checking function - implement the functions this demands */ + static void concept_check(const T& t) { - /** assignment */ - T t2 = t; + /** assignment */ + T t2 = t; - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); + /** + * Returns dimensionality of the tangent space + */ + size_t dim_ret = t.dim(); - /** - * Returns Retraction update of T - */ - T retract_ret = t.retract(gtsam::zero(dim_ret)); + /** + * Returns Retraction update of T + */ + T retract_ret = t.retract(gtsam::zero(dim_ret)); - /** - * Returns local coordinates of another object - */ - Vector localCoords_ret = t.localCoordinates(t2); - } - }; + /** + * Returns local coordinates of another object + */ + Vector localCoords_ret = t.localCoordinates(t2); + } +}; } // namespace gtsam diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index a437c9e13..16f07f5ab 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -179,14 +179,14 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) throw NegativeMatrixException(); } - Vector sqrtD = ldlt.vectorD().cwiseSqrt(); + Vector sqrtD = ldlt.vectorD().cwiseSqrt(); // FIXME: we shouldn't do sqrt in LDL if (debug) cout << "Dsqrt: " << sqrtD << endl; // U = sqrtD * L^ Matrix U = ldlt.matrixU(); // we store the permuted upper triangular matrix - ABC.block(0,0,nFrontal,nFrontal) = sqrtD.asDiagonal() * U; + ABC.block(0,0,nFrontal,nFrontal) = sqrtD.asDiagonal() * U; // FIXME: this isn't actually LDL', it's Cholesky if(debug) cout << "R:\n" << ABC.topLeftCorner(nFrontal,nFrontal) << endl; // toc(1, "ldl"); diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 155b36418..0c4689113 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -33,7 +33,7 @@ struct NegativeMatrixException : public std::exception { Matrix A; ///< The original matrix attempted to factor Matrix U; ///< The produced upper-triangular factor Matrix D; ///< The produced diagonal factor - Detail(const Matrix& _A, const Matrix& _U, const Matrix& _D) /**< Detail constructor */ : A(_A), U(_A), D(_D) {} + Detail(const Matrix& _A, const Matrix& _U, const Matrix& _D) /**< Detail constructor */ : A(_A), U(_U), D(_D) {} void print(const std::string& str = "") const { std::cout << str << "\n"; gtsam::print(A, " A: "); diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 5b335f564..12f299637 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -42,18 +42,28 @@ public: Point2(double x, double y): x_(x), y_(y) {} Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); } - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// @name Testable + /// @{ - /** print with optional string */ + /// print with optional string void print(const std::string& s = "") const; - /** equals with an tolerance, prints out message if unequal*/ + /// equals with an tolerance, prints out message if unequal bool equals(const Point2& q, double tol = 1e-9) const; - // Group requirements + /// @} + /// @name Group + /// @{ - /** "Compose", just adds the coordinates of two points. With optional derivatives */ + /// identity + inline static Point2 identity() { + return Point2(); + } + + /// "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() + inline Point2 inverse() const { return Point2(-x_, -y_); } + + /// "Compose", just adds the coordinates of two points. With optional derivatives inline Point2 compose(const Point2& p2, boost::optional H1=boost::none, boost::optional H2=boost::none) const { @@ -62,33 +72,60 @@ public: return *this + p2; } - /** identity */ - inline static Point2 identity() { - return Point2(); - } + /** operators */ + inline Point2 operator- () const {return Point2(-x_,-y_);} + inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} + inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} + inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} + inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} - /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */ - inline Point2 inverse() const { return Point2(-x_, -y_); } + /// @} + /// @name Manifold + /// @{ - // Manifold requirements + /// dimension of the variable - used to autodetect sizes + inline static size_t Dim() { return dimension; } - /** Size of the tangent space */ + /// Dimensionality of tangent space = 2 DOF inline size_t dim() const { return dimension; } - /** Updates a with tangent space delta */ + /// Updates a with tangent space delta inline Point2 retract(const Vector& v) const { return *this + Point2(v); } /// Local coordinates of manifold neighborhood around current value inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); } - /** Lie requirements */ + /// @} + /// @name Lie Group + /// @{ - /** Exponential map around identity - just create a Point2 from a vector */ + /// Exponential map around identity - just create a Point2 from a vector static inline Point2 Expmap(const Vector& v) { return Point2(v); } - /** Log map around identity - just return the Point2 as a vector */ + /// Log map around identity - just return the Point2 as a vector static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } + /// @} + /// @name Vector Operators + /// @{ + + /** norm of point */ + double norm() const; + + /** creates a unit vector */ + Point2 unit() const { return *this/norm(); } + + /** distance between two points */ + inline double dist(const Point2& p2) const { + return (p2 - *this).norm(); + } + + /** operators */ + inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} + inline void operator *= (double s) {x_*=s;y_*=s;} + inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} + + /// @} /** "Between", subtracts point coordinates */ inline Point2 between(const Point2& p2, @@ -106,27 +143,6 @@ public: /** return vectorized form (column-wise) */ Vector vector() const { return Vector_(2, x_, y_); } - /** operators */ - inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} - inline void operator *= (double s) {x_*=s;y_*=s;} - inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} - inline Point2 operator- () const {return Point2(-x_,-y_);} - inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} - inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} - inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} - inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} - - /** norm of point */ - double norm() const; - - /** creates a unit vector */ - Point2 unit() const { return *this/norm(); } - - /** distance between two points */ - inline double dist(const Point2& p2) const { - return (p2 - *this).norm(); - } - private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 2f8c9f8cc..a01f5009b 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -46,29 +46,28 @@ namespace gtsam { Point3(double x, double y, double z): x_(x), y_(y), z_(z) {} Point3(const Vector& v) : x_(v(0)), y_(v(1)), z_(v(2)) {} + /// @name Testable + /// @{ + /** print with optional string */ void print(const std::string& s = "") const; /** equals with an tolerance */ bool equals(const Point3& p, double tol = 1e-9) const; - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// @} + /// @name Group + /// @{ - /** Lie requirements */ - - /** return DOF, dimensionality of tangent space */ - inline size_t dim() const { return dimension; } - - /** identity */ + /// identity for group operation inline static Point3 identity() { return Point3(); } - /** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */ + /// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() inline Point3 inverse() const { return Point3(-x_, -y_, -z_); } - /** "Compose" - just adds coordinates of two points */ + /// "Compose" - just adds coordinates of two points inline Point3 compose(const Point3& p2, boost::optional H1=boost::none, boost::optional H2=boost::none) const { @@ -77,20 +76,58 @@ namespace gtsam { return *this + p2; } + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes + inline static size_t Dim() { return dimension; } + + /// return dimensionality of tangent space, DOF = 3 + inline size_t dim() const { return dimension; } + + /// Updates a with tangent space delta + inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); } + + /// Returns inverse retraction + inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); } + + /// @} + /// @name Lie Group + /// @{ + /** Exponential map at identity - just create a Point3 from x,y,z */ static inline Point3 Expmap(const Vector& v) { return Point3(v); } /** Log map at identity - return the x,y,z of this point */ static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } - // Manifold requirements + /// @} + /// @name Vector Operators + /// @{ - inline Point3 retract(const Vector& v) const { return compose(Expmap(v)); } + Point3 operator - () const { return Point3(-x_,-y_,-z_);} + bool operator ==(const Point3& q) const; + Point3 operator + (const Point3& q) const; + Point3 operator - (const Point3& q) const; + Point3 operator * (double s) const; + Point3 operator / (double s) const; - /** - * Returns inverse retraction - */ - inline Vector localCoordinates(const Point3& t2) const { return Logmap(t2) - Logmap(*this); } + /** distance between two points */ + double dist(const Point3& p2) const { + return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0)); + } + + /** dot product */ + double norm() const; + + /** cross product @return this x q */ + Point3 cross(const Point3 &q) const; + + /** dot product @return this * q*/ + double dot(const Point3 &q) const; + + /// @} /** Between using the default implementation */ inline Point3 between(const Point3& p2, @@ -112,19 +149,6 @@ namespace gtsam { inline double y() const {return y_;} inline double z() const {return z_;} - /** operators */ - Point3 operator - () const { return Point3(-x_,-y_,-z_);} - bool operator ==(const Point3& q) const; - Point3 operator + (const Point3& q) const; - Point3 operator - (const Point3& q) const; - Point3 operator * (double s) const; - Point3 operator / (double s) const; - - /** distance between two points */ - double dist(const Point3& p2) const { - return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0)); - } - /** add two points, add(this,q) is same as this + q */ Point3 add (const Point3 &q, boost::optional H1=boost::none, boost::optional H2=boost::none) const; @@ -133,15 +157,6 @@ namespace gtsam { Point3 sub (const Point3 &q, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** cross product @return this x q */ - Point3 cross(const Point3 &q) const; - - /** dot product @return this * q*/ - double dot(const Point3 &q) const; - - /** dot product */ - double norm() const; - private: /** Serialization function */ friend class boost::serialization::access; @@ -154,7 +169,7 @@ namespace gtsam { } }; - /** Syntactic sugar for multiplying coordinates by a scalar s*p */ + /// Syntactic sugar for multiplying coordinates by a scalar s*p inline Point3 operator*(double s, const Point3& p) { return p*s;} } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 0e4bf4824..de311249a 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -79,7 +79,6 @@ public: *this = Expmap(v); } - /// @name Testable /// @{ @@ -94,9 +93,7 @@ public: /// @{ /// identity for group operation - inline static Pose2 identity() { - return Pose2(); - } + inline static Pose2 identity() { return Pose2(); } /// inverse transformation with derivatives Pose2 inverse(boost::optional H1=boost::none) const; @@ -123,7 +120,6 @@ public: /// Dimensionality of tangent space = 3 DOF - used to autodetect sizes inline static size_t Dim() { return dimension; } - /// Dimensionality of tangent space = 3 DOF inline size_t dim() const { return dimension; } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index a7e687af1..fa6f02828 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -188,7 +188,7 @@ namespace gtsam { boost::optional H1, boost::optional H2) const { if (H1) { #ifdef CORRECT_POSE3_EXMAP - *H1 = AdjointMap(inverse(p2)); + *H1 = AdjointMap(inverse(p2)); // FIXME: this function doesn't exist with this interface #else const Rot3& R2 = p2.rotation(); const Point3& t2 = p2.translation(); @@ -216,7 +216,7 @@ namespace gtsam { Pose3 Pose3::inverse(boost::optional H1) const { if (H1) #ifdef CORRECT_POSE3_EXMAP - { *H1 = - AdjointMap(p); } + { *H1 = - AdjointMap(p); } // FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ? #else { Matrix Rt = R_.transpose(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 2fd5d7091..f03dcde06 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -65,66 +65,77 @@ namespace gtsam { /** convert to 4*4 matrix */ Matrix matrix() const; - /** print with optional string */ + /// @name Testable + /// @{ + + /// print with optional string void print(const std::string& s = "") const; - /** assert equality up to a tolerance */ + /// assert equality up to a tolerance bool equals(const Pose3& pose, double tol = 1e-9) const; - /** Compose two poses */ + /// @} + /// @name Group + /// @{ + + /// identity for group operation + static Pose3 identity() { return Pose3(); } + + /// inverse transformation with derivatives + Pose3 inverse(boost::optional H1=boost::none) const; + + ///compose this transformation onto another (first *this and then p2) + Pose3 compose(const Pose3& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + /// compose syntactic sugar Pose3 operator*(const Pose3& T) const { return Pose3(R_*T.R_, t_ + R_*T.t_); } - Pose3 transform_to(const Pose3& pose) const; + /// @} + /// @name Manifold + /// @{ - /** dimension of the variable - used to autodetect sizes */ - static size_t Dim() { return dimension; } + /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes + static size_t Dim() { return dimension; } - /** Lie requirements */ - - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space = 6 DOF size_t dim() const { return dimension; } - /** identity */ - static Pose3 identity() { - return Pose3(); - } - /** - * Derivative of inverse - */ - Pose3 inverse(boost::optional H1=boost::none) const; + /** Exponential map around another pose */ /// Retraction from R^6 to Pose3 manifold neighborhood around current pose + Pose3 retract(const Vector& d) const; - /** - * composes two poses (first (*this) then p2) - * with optional derivatives - */ - Pose3 compose(const Pose3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + /// Logarithm map around another pose T1 /// Local 6D coordinates of Pose3 manifold neighborhood around current pose + Vector localCoordinates(const Pose3& T2) const; + + /// @} + /// @name Lie Group + /// @{ + + /// Exponential map from Lie algebra se(3) to SE(3) + static Pose3 Expmap(const Vector& xi); + + /// Exponential map from SE(3) to Lie algebra se(3) + static Vector Logmap(const Pose3& p); + + /// @} + + /** syntactic sugar for transform_from */ + inline Point3 operator*(const Point3& p) const { return transform_from(p); } + + Pose3 transform_to(const Pose3& pose) const; /** receives the point in Pose coordinates and transforms it to world coordinates */ Point3 transform_from(const Point3& p, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** syntactic sugar for transform */ - inline Point3 operator*(const Point3& p) const { return transform_from(p); } - /** receives the point in world coordinates and transforms it to Pose coordinates */ Point3 transform_to(const Point3& p, boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /** Exponential map around another pose */ - Pose3 retract(const Vector& d) const; - - /** Logarithm map around another pose T1 */ - Vector localCoordinates(const Pose3& T2) const; - - /** non-approximated versions of Expmap/Logmap */ - static Pose3 Expmap(const Vector& xi); - static Vector Logmap(const Pose3& p); - /** * Return relative pose between p1 and p2, in p1 coordinate frame * as well as optionally the derivatives diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 3a8fb86f0..1a6d91b20 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -109,27 +109,25 @@ namespace gtsam { return s_; } + /// @name Testable + /// @{ + /** print */ void print(const std::string& s = "theta") const; /** equals with an tolerance */ bool equals(const Rot2& R, double tol = 1e-9) const; - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { - return dimension; - } - - /** Lie requirements */ - - /** Dimensionality of the tangent space */ - inline size_t dim() const { - return dimension; - } + /// @} + /// @name Group + /// @{ /** identity */ - inline static Rot2 identity() { - return Rot2(); + inline static Rot2 identity() { return Rot2(); } + + /** The inverse rotation - negative angle */ + Rot2 inverse() const { + return Rot2(c_, -s_); } /** Compose - make a new rotation by adding angles */ @@ -140,7 +138,41 @@ namespace gtsam { return *this * R1; } - /** Expmap around identity - create a rotation from an angle */ + /** Compose - make a new rotation by adding angles */ + Rot2 operator*(const Rot2& R) const { + return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); + } + + /** syntactic sugar for rotate */ + inline Point2 operator*(const Point2& p) const { + return rotate(p); + } + + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes + inline static size_t Dim() { + return dimension; + } + + /// Dimensionality of the tangent space, DOF = 1 + inline size_t dim() const { + return dimension; + } + + /// Updates a with tangent space delta + inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } + + /// Returns inverse retraction + inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } + + /// @} + /// @name Lie Group + /// @{ + + /// Expmap around identity - create a rotation from an angle static Rot2 Expmap(const Vector& v) { if (zero(v)) return (Rot2()); @@ -148,19 +180,23 @@ namespace gtsam { return Rot2::fromAngle(v(0)); } - /** Logmap around identity - return the angle of the rotation */ + /// Logmap around identity - return the angle of the rotation static inline Vector Logmap(const Rot2& r) { return Vector_(1, r.theta()); } - // Manifold requirements + /// @} + /// @name Vector Operators + /// @{ - inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } + /** + * Creates a unit vector as a Point2 + */ + inline Point2 unit() const { + return Point2(c_, s_); + } - /** - * Returns inverse retraction - */ - inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } + /// @} /** Between using the default implementation */ inline Rot2 between(const Rot2& p2, boost::optional H1 = @@ -176,16 +212,6 @@ namespace gtsam { /** return 2*2 transpose (inverse) rotation matrix */ Matrix transpose() const; - /** The inverse rotation - negative angle */ - Rot2 inverse() const { - return Rot2(c_, -s_); - } - - /** Compose - make a new rotation by adding angles */ - Rot2 operator*(const Rot2& R) const { - return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); - } - /** * rotate point from rotated coordinate frame to * world = R*p @@ -193,11 +219,6 @@ namespace gtsam { Point2 rotate(const Point2& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; - /** syntactic sugar for rotate */ - inline Point2 operator*(const Point2& p) const { - return rotate(p); - } - /** * rotate point from world to rotated * frame = R'*p @@ -205,12 +226,6 @@ namespace gtsam { Point2 unrotate(const Point2& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; - /** - * Creates a unit vector as a Point2 - */ - inline Point2 unit() const { - return Point2(c_, s_); - } private: /** Serialization function */ diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h index 0c17060b3..e41e01ef3 100644 --- a/gtsam/geometry/Rot3M.h +++ b/gtsam/geometry/Rot3M.h @@ -130,12 +130,76 @@ namespace gtsam { static Rot3M rodriguez(double wx, double wy, double wz) { return rodriguez(Vector_(3,wx,wy,wz));} + /// @name Testable + /// @{ + /** print */ void print(const std::string& s="R") const { gtsam::print(matrix(), s);} /** equals with an tolerance */ bool equals(const Rot3M& p, double tol = 1e-9) const; + /// @} + /// @name Group + /// @{ + + /// identity for group operation + inline static Rot3M identity() { + return Rot3M(); + } + + /// Compose two rotations i.e., R= (*this) * R2 + Rot3M compose(const Rot3M& R2, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /// rotate point from rotated coordinate frame to world = R*p + inline Point3 operator*(const Point3& p) const { return rotate(p);} + + /// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3M() + Rot3M inverse(boost::optional H1=boost::none) const { + if (H1) *H1 = -matrix(); + return Rot3M( + r1_.x(), r1_.y(), r1_.z(), + r2_.x(), r2_.y(), r2_.z(), + r3_.x(), r3_.y(), r3_.z()); + } + + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes + static size_t Dim() { return dimension; } + + /// return dimensionality of tangent space, DOF = 3 + size_t dim() const { return dimension; } + + /// Updates a with tangent space delta + Rot3M retract(const Vector& v) const { return compose(Expmap(v)); } + + /// Returns inverse retraction + Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); } + + /// @} + /// @name Lie Group + /// @{ + + /** + * Exponential map at identity - create a rotation from canonical coordinates + * using Rodriguez' formula + */ + static Rot3M Expmap(const Vector& v) { + if(zero(v)) return Rot3M(); + else return rodriguez(v); + } + + /** + * Log map at identity - return the canonical coordinates of this rotation + */ + static Vector Logmap(const Rot3M& R); + + /// @} + /** return 3*3 rotation matrix */ Matrix matrix() const; @@ -176,54 +240,6 @@ namespace gtsam { r1_.z(), r2_.z(), r3_.z()).finished()); } - /** dimension of the variable - used to autodetect sizes */ - static size_t Dim() { return dimension; } - - /** Lie requirements */ - - /** return DOF, dimensionality of tangent space */ - size_t dim() const { return dimension; } - - /** Compose two rotations i.e., R= (*this) * R2 - */ - Rot3M compose(const Rot3M& R2, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - - /** Exponential map at identity - create a rotation from canonical coordinates - * using Rodriguez' formula - */ - static Rot3M Expmap(const Vector& v) { - if(zero(v)) return Rot3M(); - else return rodriguez(v); - } - - /** identity */ - inline static Rot3M identity() { - return Rot3M(); - } - - // Log map at identity - return the canonical coordinates of this rotation - static Vector Logmap(const Rot3M& R); - - // Manifold requirements - - Rot3M retract(const Vector& v) const { return compose(Expmap(v)); } - - /** - * Returns inverse retraction - */ - Vector localCoordinates(const Rot3M& t2) const { return Logmap(between(t2)); } - - - // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3M() - Rot3M inverse(boost::optional H1=boost::none) const { - if (H1) *H1 = -matrix(); - return Rot3M( - r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()); - } - /** * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' */ @@ -236,12 +252,6 @@ namespace gtsam { return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); } - /** - * rotate point from rotated coordinate frame to - * world = R*p - */ - inline Point3 operator*(const Point3& p) const { return rotate(p);} - /** * rotate point from rotated coordinate frame to * world = R*p diff --git a/gtsam/geometry/Rot3Q.h b/gtsam/geometry/Rot3Q.h index d21f7e2fb..5986a8103 100644 --- a/gtsam/geometry/Rot3Q.h +++ b/gtsam/geometry/Rot3Q.h @@ -127,12 +127,76 @@ namespace gtsam { static Rot3Q rodriguez(double wx, double wy, double wz) { return rodriguez(Vector_(3,wx,wy,wz));} + /// @name Testable + /// @{ + /** print */ void print(const std::string& s="R") const { gtsam::print(matrix(), s);} /** equals with an tolerance */ bool equals(const Rot3Q& p, double tol = 1e-9) const; + /// @} + /// @name Group + /// @{ + + /// identity for group operation + inline static Rot3Q identity() { + return Rot3Q(); + } + + /// derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3Q() + Rot3Q inverse(boost::optional H1=boost::none) const { + if (H1) *H1 = -matrix(); + return Rot3Q(quaternion_.inverse()); + } + + /// Compose two rotations i.e., R= (*this) * R2 + Rot3Q compose(const Rot3Q& R2, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /// rotate point from rotated coordinate frame to world = R*p + inline Point3 operator*(const Point3& p) const { + Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z()); + return Point3(r(0), r(1), r(2)); + } + + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes + static size_t Dim() { return dimension; } + + /// return dimensionality of tangent space, DOF = 3 + size_t dim() const { return dimension; } + + /// Retraction from R^3 to Pose2 manifold neighborhood around current pose + Rot3Q retract(const Vector& v) const { return compose(Expmap(v)); } + + /// Returns inverse retraction + Vector localCoordinates(const Rot3Q& t2) const { return Logmap(between(t2)); } + + /// @} + /// @name Lie Group + /// @{ + + /** + * Exponential map at identity - create a rotation from canonical coordinates + * using Rodriguez' formula + */ + static Rot3Q Expmap(const Vector& v) { + if(zero(v)) return Rot3Q(); + else return rodriguez(v); + } + + /** + * Log map at identity - return the canonical coordinates of this rotation + */ + static Vector Logmap(const Rot3Q& R); + + /// @} + /** return 3*3 rotation matrix */ Matrix matrix() const; @@ -167,51 +231,6 @@ namespace gtsam { */ Quaternion toQuaternion() const { return quaternion_; } - /** dimension of the variable - used to autodetect sizes */ - static size_t Dim() { return dimension; } - - /** Lie requirements */ - - /** return DOF, dimensionality of tangent space */ - size_t dim() const { return dimension; } - - /** Compose two rotations i.e., R= (*this) * R2 - */ - Rot3Q compose(const Rot3Q& R2, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - - /** Exponential map at identity - create a rotation from canonical coordinates - * using Rodriguez' formula - */ - static Rot3Q Expmap(const Vector& v) { - if(zero(v)) return Rot3Q(); - else return rodriguez(v); - } - - /** identity */ - inline static Rot3Q identity() { - return Rot3Q(); - } - - // Log map at identity - return the canonical coordinates of this rotation - static Vector Logmap(const Rot3Q& R); - - // Manifold requirements - - Rot3Q retract(const Vector& v) const { return compose(Expmap(v)); } - - /** - * Returns inverse retraction - */ - Vector localCoordinates(const Rot3Q& t2) const { return Logmap(between(t2)); } - - - // derivative of inverse rotation R^T s.t. inverse(R)*R = Rot3Q() - Rot3Q inverse(boost::optional H1=boost::none) const { - if (H1) *H1 = -matrix(); - return Rot3Q(quaternion_.inverse()); - } - /** * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' */ @@ -222,15 +241,6 @@ namespace gtsam { /** compose two rotations */ Rot3Q operator*(const Rot3Q& R2) const { return Rot3Q(quaternion_ * R2.quaternion_); } - /** - * rotate point from rotated coordinate frame to - * world = R*p - */ - inline Point3 operator*(const Point3& p) const { - Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z()); - return Point3(r(0), r(1), r(2)); - } - /** * rotate point from rotated coordinate frame to * world = R*p diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index e493b0047..b0f2d738c 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -44,6 +44,9 @@ namespace gtsam { uL_(uL), uR_(uR), v_(v) { } + /// @name Testable + /// @{ + /** print */ void print(const std::string& s="") const; @@ -53,49 +56,53 @@ namespace gtsam { - q.v_) < tol); } - /** dimension of the variable - used to autodetect sizes */ - inline static size_t Dim() { return dimension; } + /// @} + /// @name Group + /// @{ - /** Lie requirements */ - inline size_t dim() const { return dimension; } + /// identity + inline static StereoPoint2 identity() { return StereoPoint2(); } - /** convert to vector */ - Vector vector() const { - return Vector_(3, uL_, uR_, v_); + /// inverse + inline StereoPoint2 inverse() const { + return StereoPoint2()- (*this); } - /** add two stereo points */ - StereoPoint2 operator+(const StereoPoint2& b) const { - return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); - } - - /** subtract two stereo points */ - StereoPoint2 operator-(const StereoPoint2& b) const { - return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); - } - - /* - * convenient function to get a Point2 from the left image - */ - inline Point2 point2(){ - return Point2(uL_, v_); - } - - /** "Compose", just adds the coordinates of two points. */ + /// "Compose", just adds the coordinates of two points. inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + p1; } - /** identity */ - inline static StereoPoint2 identity() { - return StereoPoint2(); + /// add two stereo points + StereoPoint2 operator+(const StereoPoint2& b) const { + return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); } - /** inverse */ - inline StereoPoint2 inverse() const { - return StereoPoint2()- (*this); + /// subtract two stereo points + StereoPoint2 operator-(const StereoPoint2& b) const { + return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); } + /// @} + /// @name Manifold + /// @{ + + /// dimension of the variable - used to autodetect sizes */ + inline static size_t Dim() { return dimension; } + + /// return dimensionality of tangent space, DOF = 3 + inline size_t dim() const { return dimension; } + + /// Updates a with tangent space delta + inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + + /// Returns inverse retraction + inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + + /// @} + /// @name Lie Group + /// @{ + /** Exponential map around identity - just create a Point2 from a vector */ static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d(0), d(1), d(2)); @@ -106,14 +113,17 @@ namespace gtsam { return p.vector(); } - // Manifold requirements + /// @}} - inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); } + /** convert to vector */ + Vector vector() const { + return Vector_(3, uL_, uR_, v_); + } - /** - * Returns inverse retraction - */ - inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); } + /** convenient function to get a Point2 from the left image */ + inline Point2 point2(){ + return Point2(uL_, v_); + } inline StereoPoint2 between(const StereoPoint2& p2) const { return gtsam::between_default(*this, p2); diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index eefa76df1..bfb1c1878 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -183,7 +183,6 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( const double dx_d_norm = result.dx_d.vector().norm(); const double newDelta = std::max(Delta, 3.0 * dx_d_norm); // Compute new Delta - if(mode == ONE_STEP_PER_ITERATION) stay = false; // If not searching, just return with the new Delta else if(mode == SEARCH_EACH_ITERATION) { @@ -217,8 +216,10 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( Delta *= 0.5; if(Delta > 1e-5) stay = true; - else + else { + if(verbose) cout << "Warning: Dog leg stopping because cannot decrease error with minimum Delta" << endl; stay = false; + } } } diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2.cpp index e4476b260..7df21bf2c 100644 --- a/gtsam/nonlinear/GaussianISAM2.cpp +++ b/gtsam/nonlinear/GaussianISAM2.cpp @@ -5,7 +5,6 @@ */ #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index 943e53aa1..d4d177815 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -9,10 +9,7 @@ #pragma once #include -#include -#include -#include -#include +#include namespace gtsam { @@ -26,15 +23,16 @@ namespace gtsam { * * @tparam VALUES The Values or TupleValues\Emph{N} that contains the * variables. + * @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph */ -template -class GaussianISAM2 : public ISAM2 { +template > +class GaussianISAM2 : public ISAM2 { public: /** Create an empty ISAM2 instance */ - GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} + GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ - GaussianISAM2() : ISAM2() {} + GaussianISAM2() : ISAM2() {} }; // optimize the BayesTree, starting from the root diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index 95a7ca346..d414e13e9 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -15,18 +15,14 @@ * @author Michael Kaess, Richard Roberts */ -#include - -#include - -#include - namespace gtsam { -template -struct ISAM2::Impl { +using namespace std; - typedef ISAM2 ISAM2Type; +template +struct ISAM2::Impl { + + typedef ISAM2 ISAM2Type; struct PartialSolveResult { typename ISAM2Type::sharedClique bayesTree; @@ -58,7 +54,7 @@ struct ISAM2::Impl { * @param factors The factors from which to extract the variables * @return The set of variables indices from the factors */ - static FastSet IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors); + static FastSet IndicesFromFactors(const Ordering& ordering, const GRAPH& factors); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -100,7 +96,7 @@ struct ISAM2::Impl { * recalculate its delta. */ static void ExpmapMasked(VALUES& values, const Permuted& delta, - const Ordering& ordering, const vector& mask, + const Ordering& ordering, const std::vector& mask, boost::optional&> invalidateIfDebug = boost::optional&>()); /** @@ -138,15 +134,15 @@ struct _VariableAdder { }; /* ************************************************************************* */ -template -void ISAM2::Impl::AddVariables( +template +void ISAM2::Impl::AddVariables( const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); if(debug) newTheta.print("The new variables are: "); // Add the new keys onto the ordering, add zeros to the delta for the new variables - vector dims(newTheta.dims(*newTheta.orderingArbitrary())); + 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(); @@ -166,8 +162,8 @@ void ISAM2::Impl::AddVariables( } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors) { +template +FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { FastSet indices; BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) { BOOST_FOREACH(const Symbol& key, factor->keys()) { @@ -178,8 +174,8 @@ FastSet ISAM2::Impl::IndicesFromFactors(const Orderin } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::CheckRelinearization(Permuted& delta, double relinearizeThreshold) { +template +FastSet ISAM2::Impl::CheckRelinearization(Permuted& delta, double relinearizeThreshold) { FastSet relinKeys; for(Index var=0; var(); @@ -191,7 +187,7 @@ FastSet ISAM2::Impl::CheckRelinearization(Permuted +template void ISAM2::Impl::FindAll(typename BayesTreeClique::shared_ptr clique, FastSet& keys, const vector& markedMask) { static const bool debug = false; // does the separator contain any of the variables? @@ -245,8 +241,8 @@ struct _SelectiveExpmapAndClear { }; /* ************************************************************************* */ -template -void ISAM2::Impl::ExpmapMasked(VALUES& values, const Permuted& delta, +template +void ISAM2::Impl::ExpmapMasked(VALUES& values, const Permuted& delta, const Ordering& ordering, const vector& mask, boost::optional&> invalidateIfDebug) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions @@ -260,9 +256,9 @@ void ISAM2::Impl::ExpmapMasked(VALUES& values, const Permut } /* ************************************************************************* */ -template -typename ISAM2::Impl::PartialSolveResult -ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, +template +typename ISAM2::Impl::PartialSolveResult +ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode) { static const bool debug = ISDEBUG("ISAM2 recalculate"); diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 461964359..4ed103615 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -21,22 +21,12 @@ #include // for operator += using namespace boost::assign; -#include -#include -#include - #include #include -#include -#include -#include #include - #include +#include -#include - -#include #include @@ -50,24 +40,24 @@ static const bool latestLast = true; static const bool structuralLast = false; /* ************************************************************************* */ -template -ISAM2::ISAM2(const ISAM2Params& params): +template +ISAM2::ISAM2(const ISAM2Params& params): delta_(Permutation(), deltaUnpermuted_), params_(params) {} /* ************************************************************************* */ -template -ISAM2::ISAM2(): +template +ISAM2::ISAM2(): delta_(Permutation(), deltaUnpermuted_) {} /* ************************************************************************* */ -template -FastList ISAM2::getAffectedFactors(const FastList& keys) const { +template +FastList ISAM2::getAffectedFactors(const FastList& keys) const { static const bool debug = false; if(debug) cout << "Getting affected factors for "; if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } if(debug) cout << endl; - FactorGraph > allAffected; + FactorGraph > allAffected; FastList indices; BOOST_FOREACH(const Index key, keys) { // const list l = nonlinearFactors_.factors(key); @@ -89,15 +79,15 @@ FastList ISAM2::getAffectedFactors(const FastList +template FactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { tic(1,"getAffectedFactors"); FastList candidates = getAffectedFactors(affectedKeys); toc(1,"getAffectedFactors"); - NonlinearFactorGraph nonlinearAffectedFactors; + GRAPH nonlinearAffectedFactors; tic(2,"affectedKeysSet"); // for fast lookup below @@ -128,9 +118,9 @@ ISAM2::relinearizeAffectedFactors(const FastList& af /* ************************************************************************* */ // find intermediate (linearized) factors from cache that are passed into the affected area -template -FactorGraph::CacheFactor> -ISAM2::getCachedBoundaryFactors(Cliques& orphans) { +template +FactorGraph::CacheFactor> +ISAM2::getCachedBoundaryFactors(Cliques& orphans) { static const bool debug = false; @@ -140,9 +130,9 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) { // find the last variable that was eliminated Index key = (*orphan)->frontals().back(); #ifndef NDEBUG -// typename BayesNet::const_iterator it = orphan->end(); -// const Conditional& lastConditional = **(--it); -// typename Conditional::const_iterator keyit = lastConditional.endParents(); +// typename BayesNet::const_iterator it = orphan->end(); +// const CONDITIONAL& lastCONDITIONAL = **(--it); +// typename CONDITIONAL::const_iterator keyit = lastCONDITIONAL.endParents(); // const Index lastKey = *(--keyit); // assert(key == lastKey); #endif @@ -154,8 +144,8 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) { return cachedBoundary; } -template -boost::shared_ptr > ISAM2::recalculate( +template +boost::shared_ptr > ISAM2::recalculate( const FastSet& markedKeys, const FastSet& structuralKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, ISAM2Result& result) { // TODO: new factors are linearized twice, the newFactors passed in are not used. @@ -174,8 +164,8 @@ boost::shared_ptr > ISAM2::recalculate( if (counter>0) { // cannot call on empty tree GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); - maxClique = cstats.maxConditionalSize; - avgClique = cstats.avgConditionalSize; + maxClique = cstats.maxCONDITIONALSize; + avgClique = cstats.avgCONDITIONALSize; numCliques = cdata.conditionalSizes.size(); nnzR = calculate_nnz(this->root()); } @@ -296,7 +286,7 @@ boost::shared_ptr > ISAM2::recalculate( toc(5,"eliminate"); tic(6,"insert"); - Base::clear(); + BayesTree::clear(); this->insert(newRoot); toc(6,"insert"); @@ -422,9 +412,9 @@ boost::shared_ptr > ISAM2::recalculate( } /* ************************************************************************* */ -template -ISAM2Result ISAM2::update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, bool force_relinearize) { +template +ISAM2Result ISAM2::update( + const GRAPH& newFactors, const Values& newTheta, bool force_relinearize) { static const bool debug = ISDEBUG("ISAM2 update"); static const bool verbose = ISDEBUG("ISAM2 update verbose"); @@ -494,14 +484,14 @@ ISAM2Result ISAM2::update( Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_); toc(6,"expmap"); - result.variablesRelinearized = markedRelinMask.size(); + result.variablesRelinearized = markedKeys.size(); #ifndef NDEBUG lastRelinVariables_ = markedRelinMask; #endif } else { -#ifndef NDEBUG result.variablesRelinearized = 0; +#ifndef NDEBUG lastRelinVariables_ = vector(ordering_.nVars(), false); #endif } @@ -560,19 +550,28 @@ ISAM2Result ISAM2::update( } /* ************************************************************************* */ -template -Values ISAM2::calculateEstimate() const { +template +VALUES ISAM2::calculateEstimate() const { // We use ExpmapMasked here instead of regular expmap because the former // handles Permuted - Values ret(theta_); + VALUES ret(theta_); vector mask(ordering_.nVars(), true); Impl::ExpmapMasked(ret, delta_, ordering_, mask); return ret; } /* ************************************************************************* */ -template -Values ISAM2::calculateBestEstimate() const { +template +template +typename KEY::Value ISAM2::calculateEstimate(const KEY& key) const { + const Index index = getOrdering()[key]; + const SubVector delta = getDelta()[index]; + return getLinearizationPoint()[key].retract(delta); +} + +/* ************************************************************************* */ +template +VALUES ISAM2::calculateBestEstimate() const { VectorValues delta(theta_.dims(ordering_)); optimize2(this->root(), delta); return theta_.retract(delta, ordering_); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index e76a33105..5e84d44d8 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -19,21 +19,8 @@ #pragma once -#include -#include -#include -#include - -#include -#include -#include -#include #include -#include -#include #include -#include -#include namespace gtsam { @@ -204,8 +191,9 @@ private: * to the constructor, then add measurements and variables as they arrive using the update() * method. At any time, calculateEstimate() may be called to obtain the current * estimate of all variables. + * */ -template +template > class ISAM2: public BayesTree > { protected: @@ -229,7 +217,7 @@ protected: Permuted delta_; /** All original nonlinear factors are stored here to use during relinearization */ - NonlinearFactorGraph nonlinearFactors_; + GRAPH nonlinearFactors_; /** @brief The current elimination ordering Symbols to Index (integer) keys. * @@ -251,6 +239,8 @@ public: typedef BayesTree > Base; ///< The BayesTree base class typedef ISAM2 This; ///< This class + typedef VALUES Values; + typedef GRAPH Graph; /** Create an empty ISAM2 instance */ ISAM2(const ISAM2Params& params); @@ -280,17 +270,27 @@ public: * (Params::relinearizeSkip). * @return An ISAM2Result struct containing information about the update */ - ISAM2Result update(const NonlinearFactorGraph& newFactors, const VALUES& newTheta, + ISAM2Result update(const GRAPH& newFactors, const VALUES& newTheta, bool force_relinearize = false); /** Access the current linearization point */ const VALUES& getLinearizationPoint() const {return theta_;} /** Compute an estimate from the incomplete linear delta computed during the last update. - * This delta is incomplete because it was not updated below wildfire_threshold. + * This delta is incomplete because it was not updated below wildfire_threshold. If only + * a single variable is needed, it is faster to call calculateEstimate(const KEY&). */ VALUES calculateEstimate() const; + /** Compute an estimate for a single variable using its incomplete linear delta computed + * during the last update. This is faster than calling the no-argument version of + * calculateEstimate, which operates on all variables. + * @param key + * @return + */ + template + typename KEY::Value calculateEstimate(const KEY& key) const; + /// @name Public members for non-typical usage //@{ @@ -305,7 +305,7 @@ public: const Permuted& getDelta() const { return delta_; } /** Access the set of nonlinear factors */ - const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } + const GRAPH& getFactorsUnsafe() const { return nonlinearFactors_; } /** Access the current ordering */ const Ordering& getOrdering() const { return ordering_; } @@ -332,3 +332,5 @@ private: }; // ISAM2 } /// namespace gtsam + +#include diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index c7f10475a..2cea72e7e 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -58,7 +58,7 @@ namespace gtsam { template void add(const F& factor) { - push_back(boost::shared_ptr(new F(factor))); + this->push_back(boost::shared_ptr(new F(factor))); } /** diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index 761f2e032..16ec47247 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -24,14 +24,13 @@ #include #include #include -#include using namespace std; using namespace gtsam; /* ************************************************************************* */ -template -void NonlinearISAM::update(const Factors& newFactors, +template +void NonlinearISAM::update(const Factors& newFactors, const Values& initialValues) { if(newFactors.size() > 0) { @@ -63,8 +62,8 @@ void NonlinearISAM::update(const Factors& newFactors, } /* ************************************************************************* */ -template -void NonlinearISAM::reorder_relinearize() { +template +void NonlinearISAM::reorder_relinearize() { // cout << "Reordering, relinearizing..." << endl; @@ -90,8 +89,8 @@ void NonlinearISAM::reorder_relinearize() { } /* ************************************************************************* */ -template -Values NonlinearISAM::estimate() const { +template +VALUES NonlinearISAM::estimate() const { if(isam_.size() > 0) return linPoint_.retract(optimize(isam_), ordering_); else @@ -99,14 +98,14 @@ Values NonlinearISAM::estimate() const { } /* ************************************************************************* */ -template -Matrix NonlinearISAM::marginalCovariance(const Symbol& key) const { +template +Matrix NonlinearISAM::marginalCovariance(const Symbol& key) const { return isam_.marginalCovariance(ordering_[key]); } /* ************************************************************************* */ -template -void NonlinearISAM::print(const std::string& s) const { +template +void NonlinearISAM::print(const std::string& s) const { cout << "ISAM - " << s << ":" << endl; cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl; isam_.print("GaussianISAM"); diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 55bff857f..8c6d2499d 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -17,7 +17,6 @@ #pragma once -#include #include #include @@ -25,11 +24,12 @@ namespace gtsam { /** * Wrapper class to manage ISAM in a nonlinear context */ -template +template > class NonlinearISAM { public: - typedef gtsam::NonlinearFactorGraph Factors; + typedef VALUES Values; + typedef GRAPH Factors; protected: @@ -101,3 +101,5 @@ public: }; } // \namespace gtsam + +#include diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index 15a7d341d..d0d5009eb 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -200,6 +200,8 @@ namespace gtsam { // The more adventurous lambda was worse too, so make lambda more conservative // and keep the same values. if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) { + if(verbosity >= Parameters::ERROR) + cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { lambda *= factor; @@ -212,6 +214,8 @@ namespace gtsam { // The more adventurous lambda was worse too, so make lambda more conservative // and keep the same values. if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) { + if(verbosity >= Parameters::ERROR) + cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { lambda *= factor; @@ -306,7 +310,7 @@ namespace gtsam { S solver(*graph_->linearize(*values_, *ordering_)); DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate( parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(), - *graph_, *values_, *ordering_, error_); + *graph_, *values_, *ordering_, error_, parameters_->verbosity_ > Parameters::ERROR); shared_values newValues(new T(values_->retract(result.dx_d, *ordering_))); cout << "newValues: " << newValues.get() << endl; return newValuesErrorLambda_(newValues, result.f_error, result.Delta); diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 5e0a6003e..4d7d76bfc 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -69,8 +69,12 @@ bool check_convergence( } bool converged = (relativeErrorTreshold && (relativeDecrease < relativeErrorTreshold)) || (absoluteDecrease < absoluteErrorTreshold); - if (verbosity >= 1 && converged) - cout << "converged" << endl; + if (verbosity >= 1 && converged) { + if(absoluteDecrease >= 0.0) + cout << "converged" << endl; + else + cout << "Warning: stopping nonlinear iterations because error increased" << endl; + } return converged; } diff --git a/gtsam/nonlinear/TupleValues-inl.h b/gtsam/nonlinear/TupleValues-inl.h index 09e510093..a9d6300c1 100644 --- a/gtsam/nonlinear/TupleValues-inl.h +++ b/gtsam/nonlinear/TupleValues-inl.h @@ -18,29 +18,6 @@ #pragma once -#include -#include - -// TupleValues instantiations for N = 1-6 -#define INSTANTIATE_TUPLE_VALUES1(Values1) \ - template class TupleValues1; - -#define INSTANTIATE_TUPLE_VALUES2(Values1, Values2) \ - template class TupleValues2; - -#define INSTANTIATE_TUPLE_VALUES3(Values1, Values2, Values3) \ - template class TupleValues3; - -#define INSTANTIATE_TUPLE_VALUES4(Values1, Values2, Values3, Values4) \ - template class TupleValues4; - -#define INSTANTIATE_TUPLE_VALUES5(Values1, Values2, Values3, Values4, Values5) \ - template class TupleValues5; - -#define INSTANTIATE_TUPLE_VALUES6(Values1, Values2, Values3, Values4, Values5, Values6) \ - template class TupleValues6; - - namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/nonlinear/TupleValues.h b/gtsam/nonlinear/TupleValues.h index a809c360f..9f1cbebae 100644 --- a/gtsam/nonlinear/TupleValues.h +++ b/gtsam/nonlinear/TupleValues.h @@ -428,9 +428,12 @@ namespace gtsam { typedef C2 Values2; typedef C3 Values3; + typedef TupleValues > > Base; + typedef TupleValues3 This; + TupleValues3() {} - TupleValues3(const TupleValues > >& values); - TupleValues3(const TupleValues3& values); + TupleValues3(const Base& values); + TupleValues3(const This& values); TupleValues3(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3); // access functions @@ -466,16 +469,18 @@ namespace gtsam { template class TupleValues5 : public TupleValues > > > > { public: - // typedefs typedef C1 Values1; typedef C2 Values2; typedef C3 Values3; typedef C4 Values4; typedef C5 Values5; + typedef TupleValues > > > > Base; + typedef TupleValues5 This; + TupleValues5() {} - TupleValues5(const TupleValues5& values); - TupleValues5(const TupleValues > > > >& values); + TupleValues5(const This& values); + TupleValues5(const Base& values); TupleValues5(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3, const Values4& cfg4, const Values5& cfg5); @@ -490,7 +495,6 @@ namespace gtsam { template class TupleValues6 : public TupleValues > > > > > { public: - // typedefs typedef C1 Values1; typedef C2 Values2; typedef C3 Values3; @@ -498,9 +502,12 @@ namespace gtsam { typedef C5 Values5; typedef C6 Values6; + typedef TupleValues > > > > > Base; + typedef TupleValues6 This; + TupleValues6() {} - TupleValues6(const TupleValues6& values); - TupleValues6(const TupleValues > > > > >& values); + TupleValues6(const This& values); + TupleValues6(const Base& values); TupleValues6(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3, const Values4& cfg4, const Values5& cfg5, const Values6& cfg6); // access functions @@ -513,3 +520,5 @@ namespace gtsam { }; } + +#include diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 4a4e1898f..e53c14944 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -28,10 +28,6 @@ #include #include -#include - -#define INSTANTIATE_VALUES(J) template class Values; - using namespace std; namespace gtsam { diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index bb45435ce..95f2409be 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -229,5 +229,7 @@ namespace gtsam { } }; -} +} // \namespace gtsam + +#include diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 0cabc5f6d..60c64b164 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -22,7 +22,7 @@ using namespace boost::assign; #include #include -#include +#include using namespace gtsam; using namespace std; diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index 9735e61d7..eb1c4614c 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -49,8 +49,9 @@ namespace gtsam { typedef NonlinearFactor1 Base; typedef PartialPriorFactor This; - Vector prior_; /// measurement on logmap parameters, in compressed form - std::vector mask_; /// flags to mask all parameters not measured + Vector prior_; ///< measurement on logmap parameters, in compressed form + std::vector mask_; ///< indices of values to constrain in compressed prior vector + Matrix H_; ///< Constant jacobian - computed at creation /** default constructor - only use for serialization */ PartialPriorFactor() {} @@ -58,10 +59,9 @@ namespace gtsam { /** * constructor with just minimum requirements for a factor - allows more * computation in the constructor. This should only be used by subclasses - * Sets the size of the mask with all values off */ PartialPriorFactor(const KEY& key, const SharedNoiseModel& model) - : Base(model, key), mask_(T::Dim(), false) {} + : Base(model, key) {} public: @@ -70,31 +70,20 @@ namespace gtsam { virtual ~PartialPriorFactor() {} - /** Full Constructor: requires mask and vector - not for typical use */ - PartialPriorFactor(const KEY& key, const std::vector& mask, - const Vector& prior, const SharedNoiseModel& model) : - Base(model, key), prior_(prior), mask_(mask) { - assert(mask_.size() == T::Dim()); // NOTE: assumes constant size variable - assert(nrTrue() == model->dim()); - assert(nrTrue() == prior_.size()); - } - /** Single Element Constructor: acts on a single parameter specified by idx */ PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedNoiseModel& model) : - Base(model, key), prior_(Vector_(1, prior)), mask_(T::Dim(), false) { + Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { assert(model->dim() == 1); - mask_[idx] = true; - assert(nrTrue() == 1); + this->fillH(); } /** Indices Constructor: specify the mask with a set of indices */ PartialPriorFactor(const KEY& key, const std::vector& mask, const Vector& prior, const SharedNoiseModel& model) : - Base(model, key), prior_(prior), mask_(T::Dim(), false) { + Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) { assert((size_t)prior_.size() == mask.size()); assert(model->dim() == (size_t) prior.size()); - setMask(mask); - assert(nrTrue() == this->dim()); + this->fillH(); } /** implement functions needed for Testable */ @@ -117,40 +106,26 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - if (H) (*H) = zeros(this->dim(), p.dim()); + if (H) (*H) = H_; // FIXME: this was originally the generic retraction - may not produce same results Vector full_logmap = T::Logmap(p); Vector masked_logmap = zero(this->dim()); - size_t masked_idx=0; - for (size_t i=0;i& mask() const { return mask_; } + const Matrix& H() const { return H_; } protected: - /** counts true elements in the mask */ - size_t nrTrue() const { - size_t result=0; + /** Constructs the jacobian matrix in place */ + void fillH() { for (size_t i=0; i& mask) { - for (size_t i=0; i(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); ar & BOOST_SERIALIZATION_NVP(mask_); + ar & BOOST_SERIALIZATION_NVP(H_); } }; // \class PartialPriorFactor diff --git a/gtsam/slam/PlanarSLAMOdometry.h b/gtsam/slam/PlanarSLAMOdometry.h index c15f2a701..941eda285 100644 --- a/gtsam/slam/PlanarSLAMOdometry.h +++ b/gtsam/slam/PlanarSLAMOdometry.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include namespace gtsam { diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 2d25d70a9..ee5649f78 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -87,6 +87,11 @@ namespace gtsam { return reprojectionError.vector(); } + /** return the measurement */ + inline const Point2 measured() const { + return z_; + } + private: /// Serialization function diff --git a/gtsam/slam/Simulated3D.cpp b/gtsam/slam/Simulated3D.cpp index 3c73b5ba9..76e65a8cf 100644 --- a/gtsam/slam/Simulated3D.cpp +++ b/gtsam/slam/Simulated3D.cpp @@ -16,17 +16,9 @@ **/ #include -#include -#include namespace gtsam { -INSTANTIATE_VALUES(simulated3D::PointKey) -INSTANTIATE_VALUES(simulated3D::PoseKey) - -using namespace simulated3D; -INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues) - namespace simulated3D { Point3 prior (const Point3& x, boost::optional H) { diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index 955d190c2..ae47a6e7d 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -19,13 +19,10 @@ #include #include #include -#include // Use planarSLAM namespace for specific SLAM instance namespace gtsam { - INSTANTIATE_VALUES(planarSLAM::PointKey) - INSTANTIATE_TUPLE_VALUES2(planarSLAM::PoseValues, planarSLAM::PointValues) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(planarSLAM::Values) INSTANTIATE_NONLINEAR_OPTIMIZER(planarSLAM::Graph, planarSLAM::Values) diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index 9219c242c..0eaaeaca1 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -16,14 +16,12 @@ **/ #include -#include #include #include // Use pose2SLAM namespace for specific SLAM instance namespace gtsam { - INSTANTIATE_VALUES(pose2SLAM::Key) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(pose2SLAM::Values) INSTANTIATE_NONLINEAR_OPTIMIZER(pose2SLAM::Graph, pose2SLAM::Values) template class NonlinearOptimizer; diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 7c7bb0d2e..4f4618896 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -16,14 +16,12 @@ **/ #include -#include #include #include // Use pose3SLAM namespace for specific SLAM instance namespace gtsam { - INSTANTIATE_VALUES(pose3SLAM::Key) INSTANTIATE_NONLINEAR_FACTOR_GRAPH(pose3SLAM::Values) INSTANTIATE_NONLINEAR_OPTIMIZER(pose3SLAM::Graph, pose3SLAM::Values) diff --git a/gtsam/slam/simulated2D.cpp b/gtsam/slam/simulated2D.cpp index 17a11dc06..ae30c6800 100644 --- a/gtsam/slam/simulated2D.cpp +++ b/gtsam/slam/simulated2D.cpp @@ -16,17 +16,9 @@ */ #include -#include -#include namespace gtsam { - INSTANTIATE_VALUES(simulated2D::PoseKey) - - using namespace simulated2D; - - INSTANTIATE_TUPLE_VALUES2(PoseValues, PointValues) - namespace simulated2D { static Matrix I = gtsam::eye(2); diff --git a/gtsam/slam/simulated2DOriented.cpp b/gtsam/slam/simulated2DOriented.cpp index 5dc1b2b22..0b68f0e46 100644 --- a/gtsam/slam/simulated2DOriented.cpp +++ b/gtsam/slam/simulated2DOriented.cpp @@ -16,13 +16,9 @@ */ #include -#include namespace gtsam { - using namespace simulated2DOriented; - - namespace simulated2DOriented { static Matrix I = gtsam::eye(3); diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 78ffeb6f8..8f09c60e7 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -34,7 +34,6 @@ using namespace std; // template definitions #include #include -#include #include namespace gtsam { diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 053f7a971..4316aadc6 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -20,7 +20,7 @@ using namespace boost; #include #include #include -#include +#include #include #include diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 36b80bd8e..f42cb6e9c 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -20,7 +20,7 @@ using namespace boost; #include #include #include -#include +#include #include #include diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 54f3efa38..1eb62ec93 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -107,8 +107,7 @@ TEST(Pose3Graph, partial_prior_height) { EXPECT(assert_equal(expA, actA)); pose3SLAM::Graph graph; -// graph.add(height); // FAIL - on compile, can't initialize a reference? - graph.push_back(boost::shared_ptr(new Partial(height))); + graph.add(height); pose3SLAM::Values values; values.insert(key, init); @@ -163,7 +162,7 @@ TEST(Pose3Graph, partial_prior_xy) { EXPECT(assert_equal(expA, actA)); pose3SLAM::Graph graph; - graph.push_back(Partial::shared_ptr(new Partial(priorXY))); + graph.add(priorXY); pose3SLAM::Values values; values.insert(key, init); diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index 23244f5f9..5a8200dd0 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -16,12 +16,11 @@ */ #include -#include #include #include namespace gtsam { - INSTANTIATE_TUPLE_VALUES2(visualSLAM::PoseValues, visualSLAM::PointValues) + INSTANTIATE_NONLINEAR_FACTOR_GRAPH(visualSLAM::Values) INSTANTIATE_NONLINEAR_OPTIMIZER(visualSLAM::Graph, visualSLAM::Values) diff --git a/myconfigure.ardrone b/myconfigure.ardrone new file mode 100755 index 000000000..117bcdadf --- /dev/null +++ b/myconfigure.ardrone @@ -0,0 +1 @@ +../configure --build=i686-pc-linux-gnu --host=arm-none-linux-gnueabi -prefix=/usr CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 3027ab02f..3a946858a 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include using namespace gtsam; diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 1bd46f12f..b4e04603f 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -27,7 +27,6 @@ using namespace boost::assign; #define GTSAM_MAGIC_GAUSSIAN 3 #include -#include #include #include diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 31e870f00..501bb9db8 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -24,8 +24,6 @@ #include #include -#include - using namespace std; using namespace gtsam; diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 01d2920ad..9917273b9 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -35,7 +35,6 @@ #include #include #include -#include using namespace std; using namespace gtsam; diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index a7f3bb285..b90e98625 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include using namespace gtsam; diff --git a/tests/testRot3QOptimization.cpp b/tests/testRot3QOptimization.cpp index b94cad3a5..d46c30ea4 100644 --- a/tests/testRot3QOptimization.cpp +++ b/tests/testRot3QOptimization.cpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/tests/testTupleValues.cpp b/tests/testTupleValues.cpp index b75e98102..4ba2e3a5f 100644 --- a/tests/testTupleValues.cpp +++ b/tests/testTupleValues.cpp @@ -30,7 +30,7 @@ #include #include -#include +#include using namespace gtsam; using namespace std; From fd5b040385dee66a99d4f74ec3a2736bfba31967 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 5 Dec 2011 02:28:09 +0000 Subject: [PATCH 05/17] (in branch) in progress refactoring for incremental dogleg --- gtsam/inference/BayesTree-inl.h | 229 -------------------- gtsam/inference/BayesTree.h | 132 ++---------- gtsam/inference/BayesTreeCliqueBase-inl.h | 251 ++++++++++++++++++++++ gtsam/inference/BayesTreeCliqueBase.h | 153 +++++++++++++ gtsam/linear/GaussianConditional.h | 8 +- gtsam/nonlinear/DoglegOptimizerImpl.cpp | 14 +- gtsam/nonlinear/DoglegOptimizerImpl.h | 5 +- gtsam/nonlinear/ISAM2.h | 68 ++---- 8 files changed, 451 insertions(+), 409 deletions(-) create mode 100644 gtsam/inference/BayesTreeCliqueBase-inl.h create mode 100644 gtsam/inference/BayesTreeCliqueBase.h diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index cddd642f0..c003102e4 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -42,79 +42,6 @@ namespace gtsam { using namespace std; - /* ************************************************************************* */ - template - void BayesTreeClique::assertInvariants() const { -#ifndef NDEBUG - // We rely on the keys being sorted -// FastVector sortedUniqueKeys(conditional_->begin(), conditional_->end()); -// std::sort(sortedUniqueKeys.begin(), sortedUniqueKeys.end()); -// std::unique(sortedUniqueKeys.begin(), sortedUniqueKeys.end()); -// assert(sortedUniqueKeys.size() == conditional_->size() && -// std::equal(sortedUniqueKeys.begin(), sortedUniqueKeys.end(), conditional_->begin())); -#endif - } - - /* ************************************************************************* */ - template - BayesTreeClique::BayesTreeClique(const sharedConditional& conditional) : conditional_(conditional) { - assertInvariants(); - } - - /* ************************************************************************* */ - template - void BayesTreeClique::print(const string& s) const { - conditional_->print(s); - } - - /* ************************************************************************* */ - template - size_t BayesTreeClique::treeSize() const { - size_t size = 1; - BOOST_FOREACH(const shared_ptr& child, children_) - size += child->treeSize(); - return size; - } - - /* ************************************************************************* */ - template - void BayesTreeClique::printTree(const string& indent) const { - print(indent); - BOOST_FOREACH(const shared_ptr& child, children_) - child->printTree(indent+" "); - } - - /* ************************************************************************* */ - template - void BayesTreeClique::permuteWithInverse(const Permutation& inversePermutation) { - conditional_->permuteWithInverse(inversePermutation); - BOOST_FOREACH(const shared_ptr& child, children_) { - child->permuteWithInverse(inversePermutation); - } - assertInvariants(); - } - - /* ************************************************************************* */ - template - bool BayesTreeClique::permuteSeparatorWithInverse(const Permutation& inversePermutation) { - bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation); -#ifndef NDEBUG - if(!changed) { - BOOST_FOREACH(Index& separatorKey, conditional_->parents()) { assert(separatorKey == inversePermutation[separatorKey]); } - BOOST_FOREACH(const shared_ptr& child, children_) { - assert(child->permuteSeparatorWithInverse(inversePermutation) == false); - } - } -#endif - if(changed) { - BOOST_FOREACH(const shared_ptr& child, children_) { - (void)child->permuteSeparatorWithInverse(inversePermutation); - } - } - assertInvariants(); - return changed; - } - /* ************************************************************************* */ template typename BayesTree::CliqueData @@ -208,162 +135,6 @@ namespace gtsam { return stats; } - /* ************************************************************************* */ - // The shortcut density is a conditional P(S|R) of the separator of this - // clique on the root. We can compute it recursively from the parent shortcut - // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p - /* ************************************************************************* */ - template - BayesNet BayesTreeClique::shortcut(shared_ptr R, - Eliminate function) { - - static const bool debug = false; - - // A first base case is when this clique or its parent is the root, - // in which case we return an empty Bayes net. - - shared_ptr parent(parent_.lock()); - - if (R.get()==this || parent==R) { - BayesNet empty; - return empty; - } - - // The root conditional - FactorGraph p_R(BayesNet(R->conditional())); - - // The parent clique has a CONDITIONAL for each frontal node in Fp - // so we can obtain P(Fp|Sp) in factor graph form - FactorGraph p_Fp_Sp(BayesNet(parent->conditional())); - - // If not the base case, obtain the parent shortcut P(Sp|R) as factors - FactorGraph p_Sp_R(parent->shortcut(R, function)); - - // now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R) - FactorGraph p_Cp_R; - p_Cp_R.push_back(p_R); - p_Cp_R.push_back(p_Fp_Sp); - p_Cp_R.push_back(p_Sp_R); - - // Eliminate into a Bayes net with ordering designed to integrate out - // any variables not in *our* separator. Variables to integrate out must be - // eliminated first hence the desired ordering is [Cp\S S]. - // However, an added wrinkle is that Cp might overlap with the root. - // Keys corresponding to the root should not be added to the ordering at all. - - if(debug) { - p_R.print("p_R: "); - p_Fp_Sp.print("p_Fp_Sp: "); - p_Sp_R.print("p_Sp_R: "); - } - - // We want to factor into a conditional of the clique variables given the - // root and the marginal on the root, integrating out all other variables. - // The integrands include any parents of this clique and the variables of - // the parent clique. - FastSet variablesAtBack; - FastSet separator; - size_t uniqueRootVariables = 0; - BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) { - variablesAtBack.insert(separatorIndex); - separator.insert(separatorIndex); - if(debug) cout << "At back (this): " << separatorIndex << endl; - } - BOOST_FOREACH(const Index key, R->conditional()->keys()) { - if(variablesAtBack.insert(key).second) - ++ uniqueRootVariables; - if(debug) cout << "At back (root): " << key << endl; - } - - Permutation toBack = Permutation::PushToBack( - vector(variablesAtBack.begin(), variablesAtBack.end()), - R->conditional()->lastFrontalKey() + 1); - Permutation::shared_ptr toBackInverse(toBack.inverse()); - BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) { - factor->permuteWithInverse(*toBackInverse); } - typename BayesNet::shared_ptr eliminated(EliminationTree< - FactorType>::Create(p_Cp_R)->eliminate(function)); - - // Take only the conditionals for p(S|R). We check for each variable being - // in the separator set because if some separator variables overlap with - // root variables, we cannot rely on the number of root variables, and also - // want to include those variables in the conditional. - BayesNet p_S_R; - BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, *eliminated) { - assert(conditional->nrFrontals() == 1); - if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) { - if(debug) - conditional->print("Taking C|R conditional: "); - p_S_R.push_front(conditional); - } - if(p_S_R.size() == separator.size()) - break; - } - - // Undo the permutation - if(debug) toBack.print("toBack: "); - p_S_R.permuteWithInverse(toBack); - - // return the parent shortcut P(Sp|R) - assertInvariants(); - return p_S_R; - } - - /* ************************************************************************* */ - // P(C) = \int_R P(F|S) P(S|R) P(R) - // TODO: Maybe we should integrate given parent marginal P(Cp), - // \int(Cp\S) P(F|S)P(S|Cp)P(Cp) - // Because the root clique could be very big. - /* ************************************************************************* */ - template - FactorGraph BayesTreeClique::marginal( - shared_ptr R, Eliminate function) { - // If we are the root, just return this root - // NOTE: immediately cast to a factor graph - BayesNet bn(R->conditional()); - if (R.get()==this) return bn; - - // Combine P(F|S), P(S|R), and P(R) - BayesNet p_FSR = this->shortcut(R, function); - p_FSR.push_front(this->conditional()); - p_FSR.push_back(R->conditional()); - - assertInvariants(); - GenericSequentialSolver solver(p_FSR); - return *solver.jointFactorGraph(conditional_->keys(), function); - } - - /* ************************************************************************* */ - // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R) - /* ************************************************************************* */ - template - FactorGraph BayesTreeClique::joint( - shared_ptr C2, shared_ptr R, Eliminate function) { - // For now, assume neither is the root - - // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R) - FactorGraph joint; - if (!isRoot()) joint.push_back(this->conditional()->toFactor()); // P(F1|S1) - if (!isRoot()) joint.push_back(shortcut(R, function)); // P(S1|R) - if (!C2->isRoot()) joint.push_back(C2->conditional()->toFactor()); // P(F2|S2) - if (!C2->isRoot()) joint.push_back(C2->shortcut(R, function)); // P(S2|R) - joint.push_back(R->conditional()->toFactor()); // P(R) - - // Find the keys of both C1 and C2 - vector keys1(conditional_->keys()); - vector keys2(C2->conditional_->keys()); - FastSet keys12; - keys12.insert(keys1.begin(), keys1.end()); - keys12.insert(keys2.begin(), keys2.end()); - - // Calculate the marginal - vector keys12vector; keys12vector.reserve(keys12.size()); - keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end()); - assertInvariants(); - GenericSequentialSolver solver(joint); - return *solver.jointFactorGraph(keys12vector, function); - } - /* ************************************************************************* */ template void BayesTree::Cliques::print(const std::string& s) const { diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 45dea6e49..2f6b9735e 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -24,11 +24,13 @@ #include #include #include +#include #include #include #include #include +#include #include namespace gtsam { @@ -196,6 +198,8 @@ namespace gtsam { /** return root clique */ const sharedClique& root() const { return root_; } + + /** Access the root clique (non-const version) */ sharedClique& root() { return root_; } /** find the clique to which key belongs */ @@ -207,24 +211,20 @@ namespace gtsam { CliqueData getCliqueData() const; /** return marginal on any variable */ - typename FactorType::shared_ptr marginalFactor(Index key, - Eliminate function) const; + typename FactorType::shared_ptr marginalFactor(Index key, Eliminate function) const; /** * return marginal on any variable, as a Bayes Net * NOTE: this function calls marginal, and then eliminates it into a Bayes Net * This is more expensive than the above function */ - typename BayesNet::shared_ptr marginalBayesNet(Index key, - Eliminate function) const; + typename BayesNet::shared_ptr marginalBayesNet(Index key, Eliminate function) const; /** return joint on two variables */ - typename FactorGraph::shared_ptr joint(Index key1, Index key2, - Eliminate function) const; + typename FactorGraph::shared_ptr joint(Index key1, Index key2, Eliminate function) const; /** return joint on two variables as a BayesNet */ - typename BayesNet::shared_ptr jointBayesNet(Index key1, - Index key2, Eliminate function) const; + typename BayesNet::shared_ptr jointBayesNet(Index key1, Index key2, Eliminate function) const; /** * Read only with side effects @@ -310,120 +310,24 @@ namespace gtsam { * extra data along with the clique. */ template - struct BayesTreeClique { - - }; - - - /* ************************************************************************* */ - /** - * This is the base class for BayesTree cliques. The default and standard - * derived type is BayesTreeClique, but some algorithms, like iSAM2, use a - * different clique type in order to store extra data along with the clique. - */ - template - struct BayesTreeCliqueBase { - - protected: - void assertInvariants() const; - + struct BayesTreeClique : public BayesTreeCliqueBase > { public: - typedef BayesTreeClique This; - typedef DERIVED DerivedType; - typedef typename DERIVED::ConditionalType ConditionalType; - typedef boost::shared_ptr sharedConditional; - typedef typename boost::shared_ptr shared_ptr; - typedef typename boost::weak_ptr weak_ptr; - typedef typename ConditionalType::FactorType FactorType; - typedef typename FactorGraph::Eliminate Eliminate; - - sharedConditional conditional_; - weak_ptr parent_; - std::list children_; - - friend class BayesTree; - - /** Default constructor */ - BayesTreeCliqueBase() {} - - /** Construct from a conditional, leaving parent and child pointers uninitialized */ - BayesTreeCliqueBase(const sharedConditional& conditional); - - virtual ~BayesTreeCliqueBase() {} - - /** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */ - static shared_ptr Create(const sharedConditional& conditional) { return shared_ptr(new BayesTreeClique(conditional)); } - - /** Construct shared_ptr from a FactorGraph::EliminationResult. In this class - * the conditional part is kept and the factor part is ignored, but in derived clique - * types, such as ISAM2Clique, the factor part is kept as a cached factor. - * @param - */ - template - static shared_ptr Create(const RESULT& result) { return Create(result.first); } - - /** print this node */ - void print(const std::string& s = "") const; - - /** The arrow operator accesses the conditional */ - const ConditionalType* operator->() const { return conditional_.get(); } - - /** The arrow operator accesses the conditional */ - ConditionalType* operator->() { return conditional_.get(); } - - /** Access the conditional */ - const sharedConditional& conditional() const { return conditional_; } - - /** is this the root of a Bayes tree ? */ - inline bool isRoot() const { return parent_.expired(); } - - /** return the const reference of children */ - std::list& children() { return children_; } - const std::list& children() const { return children_; } - - /** The size of subtree rooted at this clique, i.e., nr of Cliques */ - size_t treeSize() const; - - /** print this node and entire subtree below it */ - void printTree(const std::string& indent="") const; - - /** Permute the variables in the whole subtree rooted at this clique */ - void permuteWithInverse(const Permutation& inversePermutation); - - /** Permute variables when they only appear in the separators. In this - * case the running intersection property will be used to prevent always - * traversing the whole tree. Returns whether any separator variables in - * this subtree were reordered. - */ - bool permuteSeparatorWithInverse(const Permutation& inversePermutation); - - /** return the conditional P(S|Root) on the separator given the root */ - // TODO: create a cached version - BayesNet shortcut(shared_ptr root, Eliminate function); - - /** return the marginal P(C) of the clique */ - FactorGraph marginal(shared_ptr root, Eliminate function); - - /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ - FactorGraph joint(shared_ptr C2, shared_ptr root, Eliminate function); - - bool equals(const This& other, double tol=1e-9) const { - return (!conditional_ && !other.conditional()) || - conditional_->equals(*(other.conditional()), tol); - } + typedef CONDITIONAL ConditionalType; + typedef BayesTreeClique This; + typedef BayesTreeCliqueBase Base; + typedef boost::shared_ptr shared_ptr; + BayesTreeClique() {} + BayesTreeClique(const sharedConditional& conditional) : Base(conditional) {} private: /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(conditional_); - ar & BOOST_SERIALIZATION_NVP(parent_); - ar & BOOST_SERIALIZATION_NVP(children_); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } + }; - }; // \struct Clique - - +#include } /// namespace gtsam diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h new file mode 100644 index 000000000..2956a6cc2 --- /dev/null +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -0,0 +1,251 @@ +/* ---------------------------------------------------------------------------- + + * 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 BayesTreeCliqueBase + * @brief Base class for cliques of a BayesTree + * @author Richard Roberts and Frank Dellaert + */ + +#pragma once + +namespace gtsam { + + /* ************************************************************************* */ + template + void BayesTreeClique::assertInvariants() const { +#ifndef NDEBUG + // We rely on the keys being sorted +// FastVector sortedUniqueKeys(conditional_->begin(), conditional_->end()); +// std::sort(sortedUniqueKeys.begin(), sortedUniqueKeys.end()); +// std::unique(sortedUniqueKeys.begin(), sortedUniqueKeys.end()); +// assert(sortedUniqueKeys.size() == conditional_->size() && +// std::equal(sortedUniqueKeys.begin(), sortedUniqueKeys.end(), conditional_->begin())); +#endif + } + + /* ************************************************************************* */ + template + BayesTreeClique::BayesTreeClique(const sharedConditional& conditional) : conditional_(conditional) { + assertInvariants(); + } + + /* ************************************************************************* */ + template + void BayesTreeClique::print(const string& s) const { + conditional_->print(s); + } + + /* ************************************************************************* */ + template + size_t BayesTreeClique::treeSize() const { + size_t size = 1; + BOOST_FOREACH(const shared_ptr& child, children_) + size += child->treeSize(); + return size; + } + + /* ************************************************************************* */ + template + void BayesTreeClique::printTree(const string& indent) const { + print(indent); + BOOST_FOREACH(const shared_ptr& child, children_) + child->printTree(indent+" "); + } + + /* ************************************************************************* */ + template + void BayesTreeClique::permuteWithInverse(const Permutation& inversePermutation) { + conditional_->permuteWithInverse(inversePermutation); + BOOST_FOREACH(const shared_ptr& child, children_) { + child->permuteWithInverse(inversePermutation); + } + assertInvariants(); + } + + /* ************************************************************************* */ + template + bool BayesTreeClique::permuteSeparatorWithInverse(const Permutation& inversePermutation) { + bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation); +#ifndef NDEBUG + if(!changed) { + BOOST_FOREACH(Index& separatorKey, conditional_->parents()) { assert(separatorKey == inversePermutation[separatorKey]); } + BOOST_FOREACH(const shared_ptr& child, children_) { + assert(child->permuteSeparatorWithInverse(inversePermutation) == false); + } + } +#endif + if(changed) { + BOOST_FOREACH(const shared_ptr& child, children_) { + (void)child->permuteSeparatorWithInverse(inversePermutation); + } + } + assertInvariants(); + return changed; + } + + /* ************************************************************************* */ + // The shortcut density is a conditional P(S|R) of the separator of this + // clique on the root. We can compute it recursively from the parent shortcut + // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p + /* ************************************************************************* */ + template + BayesNet BayesTreeClique::shortcut(shared_ptr R, + Eliminate function) { + + static const bool debug = false; + + // A first base case is when this clique or its parent is the root, + // in which case we return an empty Bayes net. + + shared_ptr parent(parent_.lock()); + + if (R.get()==this || parent==R) { + BayesNet empty; + return empty; + } + + // The root conditional + FactorGraph p_R(BayesNet(R->conditional())); + + // The parent clique has a CONDITIONAL for each frontal node in Fp + // so we can obtain P(Fp|Sp) in factor graph form + FactorGraph p_Fp_Sp(BayesNet(parent->conditional())); + + // If not the base case, obtain the parent shortcut P(Sp|R) as factors + FactorGraph p_Sp_R(parent->shortcut(R, function)); + + // now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R) + FactorGraph p_Cp_R; + p_Cp_R.push_back(p_R); + p_Cp_R.push_back(p_Fp_Sp); + p_Cp_R.push_back(p_Sp_R); + + // Eliminate into a Bayes net with ordering designed to integrate out + // any variables not in *our* separator. Variables to integrate out must be + // eliminated first hence the desired ordering is [Cp\S S]. + // However, an added wrinkle is that Cp might overlap with the root. + // Keys corresponding to the root should not be added to the ordering at all. + + if(debug) { + p_R.print("p_R: "); + p_Fp_Sp.print("p_Fp_Sp: "); + p_Sp_R.print("p_Sp_R: "); + } + + // We want to factor into a conditional of the clique variables given the + // root and the marginal on the root, integrating out all other variables. + // The integrands include any parents of this clique and the variables of + // the parent clique. + FastSet variablesAtBack; + FastSet separator; + size_t uniqueRootVariables = 0; + BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) { + variablesAtBack.insert(separatorIndex); + separator.insert(separatorIndex); + if(debug) cout << "At back (this): " << separatorIndex << endl; + } + BOOST_FOREACH(const Index key, R->conditional()->keys()) { + if(variablesAtBack.insert(key).second) + ++ uniqueRootVariables; + if(debug) cout << "At back (root): " << key << endl; + } + + Permutation toBack = Permutation::PushToBack( + vector(variablesAtBack.begin(), variablesAtBack.end()), + R->conditional()->lastFrontalKey() + 1); + Permutation::shared_ptr toBackInverse(toBack.inverse()); + BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) { + factor->permuteWithInverse(*toBackInverse); } + typename BayesNet::shared_ptr eliminated(EliminationTree< + FactorType>::Create(p_Cp_R)->eliminate(function)); + + // Take only the conditionals for p(S|R). We check for each variable being + // in the separator set because if some separator variables overlap with + // root variables, we cannot rely on the number of root variables, and also + // want to include those variables in the conditional. + BayesNet p_S_R; + BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, *eliminated) { + assert(conditional->nrFrontals() == 1); + if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) { + if(debug) + conditional->print("Taking C|R conditional: "); + p_S_R.push_front(conditional); + } + if(p_S_R.size() == separator.size()) + break; + } + + // Undo the permutation + if(debug) toBack.print("toBack: "); + p_S_R.permuteWithInverse(toBack); + + // return the parent shortcut P(Sp|R) + assertInvariants(); + return p_S_R; + } + + /* ************************************************************************* */ + // P(C) = \int_R P(F|S) P(S|R) P(R) + // TODO: Maybe we should integrate given parent marginal P(Cp), + // \int(Cp\S) P(F|S)P(S|Cp)P(Cp) + // Because the root clique could be very big. + /* ************************************************************************* */ + template + FactorGraph BayesTreeClique::marginal( + shared_ptr R, Eliminate function) { + // If we are the root, just return this root + // NOTE: immediately cast to a factor graph + BayesNet bn(R->conditional()); + if (R.get()==this) return bn; + + // Combine P(F|S), P(S|R), and P(R) + BayesNet p_FSR = this->shortcut(R, function); + p_FSR.push_front(this->conditional()); + p_FSR.push_back(R->conditional()); + + assertInvariants(); + GenericSequentialSolver solver(p_FSR); + return *solver.jointFactorGraph(conditional_->keys(), function); + } + + /* ************************************************************************* */ + // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R) + /* ************************************************************************* */ + template + FactorGraph BayesTreeClique::joint( + shared_ptr C2, shared_ptr R, Eliminate function) { + // For now, assume neither is the root + + // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R) + FactorGraph joint; + if (!isRoot()) joint.push_back(this->conditional()->toFactor()); // P(F1|S1) + if (!isRoot()) joint.push_back(shortcut(R, function)); // P(S1|R) + if (!C2->isRoot()) joint.push_back(C2->conditional()->toFactor()); // P(F2|S2) + if (!C2->isRoot()) joint.push_back(C2->shortcut(R, function)); // P(S2|R) + joint.push_back(R->conditional()->toFactor()); // P(R) + + // Find the keys of both C1 and C2 + vector keys1(conditional_->keys()); + vector keys2(C2->conditional_->keys()); + FastSet keys12; + keys12.insert(keys1.begin(), keys1.end()); + keys12.insert(keys2.begin(), keys2.end()); + + // Calculate the marginal + vector keys12vector; keys12vector.reserve(keys12.size()); + keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end()); + assertInvariants(); + GenericSequentialSolver solver(joint); + return *solver.jointFactorGraph(keys12vector, function); + } + +} diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h new file mode 100644 index 000000000..549814312 --- /dev/null +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -0,0 +1,153 @@ +/* ---------------------------------------------------------------------------- + + * 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 BayesTreeCliqueBase + * @brief Base class for cliques of a BayesTree + * @author Richard Roberts and Frank Dellaert + */ + +#pragma once + +#include +#include + +#include +#include +#include + +namespace gtsam { + + /** + * This is the base class for BayesTree cliques. The default and standard + * derived type is BayesTreeClique, but some algorithms, like iSAM2, use a + * different clique type in order to store extra data along with the clique. + * + * This class is templated on the derived class (i.e. the curiously recursive + * template pattern). The advantage of this over using virtual classes is + * that it avoids the need for casting to get the derived type. This is + * possible because all cliques in a BayesTree are the same type - if they + * were not then we'd need a virtual class. + * + * @tparam The derived clique type. + */ + template + struct BayesTreeCliqueBase { + + protected: + void assertInvariants() const; + + /** Default constructor */ + BayesTreeCliqueBase() {} + + /** Construct from a conditional, leaving parent and child pointers uninitialized */ + BayesTreeCliqueBase(const sharedConditional& conditional); + + public: + typedef BayesTreeClique This; + typedef DERIVED DerivedType; + typedef typename DERIVED::ConditionalType ConditionalType; + typedef boost::shared_ptr sharedConditional; + typedef typename boost::shared_ptr shared_ptr; + typedef typename boost::weak_ptr weak_ptr; + typedef typename boost::shared_ptr derived_ptr; + typedef typename boost::weak_ptr derived_weak_ptr; + typedef typename ConditionalType::FactorType FactorType; + typedef typename FactorGraph::Eliminate Eliminate; + + sharedConditional conditional_; + derived_weak_ptr parent_; + std::list children_; + + /** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */ + static derived_ptr Create(const sharedConditional& conditional) { return boost::make_shared(conditional); } + + /** Construct shared_ptr from a FactorGraph::EliminationResult. In this class + * the conditional part is kept and the factor part is ignored, but in derived clique + * types, such as ISAM2Clique, the factor part is kept as a cached factor. + * @param An elimination result, which is a pair + */ + static derived_ptr Create(const std::pair >& result) { return boost::make_shared(result); } + + /** print this node */ + void print(const std::string& s = "") const; + + /** The arrow operator accesses the conditional */ + const ConditionalType* operator->() const { return conditional_.get(); } + + /** The arrow operator accesses the conditional */ + ConditionalType* operator->() { return conditional_.get(); } + + /** Access the conditional */ + const sharedConditional& conditional() const { return conditional_; } + + /** is this the root of a Bayes tree ? */ + inline bool isRoot() const { return parent_.expired(); } + + /** return the const reference of children */ + std::list& children() { return children_; } + const std::list& children() const { return children_; } + + /** The size of subtree rooted at this clique, i.e., nr of Cliques */ + size_t treeSize() const; + + /** print this node and entire subtree below it */ + void printTree(const std::string& indent="") const; + + /** Permute the variables in the whole subtree rooted at this clique */ + void permuteWithInverse(const Permutation& inversePermutation); + + /** Permute variables when they only appear in the separators. In this + * case the running intersection property will be used to prevent always + * traversing the whole tree. Returns whether any separator variables in + * this subtree were reordered. + */ + bool permuteSeparatorWithInverse(const Permutation& inversePermutation); + + /** return the conditional P(S|Root) on the separator given the root */ + // TODO: create a cached version + BayesNet shortcut(shared_ptr root, Eliminate function); + + /** return the marginal P(C) of the clique */ + FactorGraph marginal(shared_ptr root, Eliminate function); + + /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ + FactorGraph joint(shared_ptr C2, shared_ptr root, Eliminate function); + + bool equals(const This& other, double tol=1e-9) const { + return (!conditional_ && !other.conditional()) || + conditional_->equals(*(other.conditional()), tol); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(conditional_); + ar & BOOST_SERIALIZATION_NVP(parent_); + ar & BOOST_SERIALIZATION_NVP(children_); + } + + }; // \struct Clique + + template + typename BayesTreeCliqueBase::derived_ptr asDerived(const BayesTreeCliqueBase& base) { +#ifndef NDEBUG + return boost::dynamic_pointer_cast(base); +#else + return boost::static_pointer_cast(base); +#endif + } + +#include + +} diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 54f96efe2..2bbfc2f22 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -52,8 +52,8 @@ public: typedef boost::shared_ptr shared_ptr; /** Store the conditional matrix as upper-triangular column-major */ - typedef Matrix AbMatrix; - typedef VerticalBlockView rsd_type; + typedef Matrix RdMatrix; + typedef VerticalBlockView rsd_type; typedef rsd_type::Block r_type; typedef rsd_type::constBlock const_r_type; @@ -72,7 +72,7 @@ protected: * Use R*permutation_ to get back the correct non-permuted order, * for example when converting to the Jacobian * */ - AbMatrix matrix_; + RdMatrix matrix_; rsd_type rsd_; /** vector of standard deviations */ @@ -169,7 +169,7 @@ public: protected: - const AbMatrix& matrix() const { return matrix_; } + const RdMatrix& matrix() const { return matrix_; } const rsd_type& rsd() const { return rsd_; } public: diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index 32ef1b0ac..9316e0839 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -9,28 +9,28 @@ namespace gtsam { /* ************************************************************************* */ VectorValues DoglegOptimizerImpl::ComputeDoglegPoint( - double Delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose) { + double Delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose) { // Get magnitude of each update and find out which segment Delta falls in assert(Delta >= 0.0); double DeltaSq = Delta*Delta; - double x_u_norm_sq = x_u.vector().squaredNorm(); - double x_n_norm_sq = x_n.vector().squaredNorm(); + double x_u_norm_sq = dx_u.vector().squaredNorm(); + double x_n_norm_sq = dx_n.vector().squaredNorm(); if(verbose) cout << "Steepest descent magnitude " << sqrt(x_u_norm_sq) << ", Newton's method magnitude " << sqrt(x_n_norm_sq) << endl; if(DeltaSq < x_u_norm_sq) { // Trust region is smaller than steepest descent update - VectorValues x_d = VectorValues::SameStructure(x_u); - x_d.vector() = x_u.vector() * sqrt(DeltaSq / x_u_norm_sq); + VectorValues x_d = VectorValues::SameStructure(dx_u); + x_d.vector() = dx_u.vector() * sqrt(DeltaSq / x_u_norm_sq); if(verbose) cout << "In steepest descent region with fraction " << sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl; return x_d; } else if(DeltaSq < x_n_norm_sq) { // Trust region boundary is between steepest descent point and Newton's method point - return ComputeBlend(Delta, x_u, x_n); + return ComputeBlend(Delta, dx_u, dx_n); } else { assert(DeltaSq >= x_n_norm_sq); if(verbose) cout << "In pure Newton's method region" << endl; // Trust region is larger than Newton's method point - return x_n; + return dx_n; } } diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index bfb1c1878..5f5801918 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -95,10 +95,11 @@ struct DoglegOptimizerImpl { * GaussianBayesNet, containing GaussianConditional s. * * @param Delta The trust region radius - * @param bayesNet The Bayes' net \f$ (R,d) \f$ as described above. + * @param dx_u The steepest descent point, i.e. the Cauchy point + * @param dx_n The Gauss-Newton point * @return The dogleg point \f$ \delta x_d \f$ */ - static VectorValues ComputeDoglegPoint(double Delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose=false); + static VectorValues ComputeDoglegPoint(double Delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose=false); /** Compute the minimizer \f$ \delta x_u \f$ of the line search along the gradient direction \f$ g \f$ of * the function diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 5e84d44d8..3e3d34996 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -106,71 +106,32 @@ struct ISAM2Result { }; template -struct ISAM2Clique : public BayesTreeClique { +struct ISAM2Clique : public BayesTreeCliqueBase > { typedef ISAM2Clique This; - typedef BayesTreeClique Base; - + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; typename Base::FactorType::shared_ptr cachedFactor_; + Vector gradientContribution_; /** Access the cached factor */ typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } - /** Construct from an elimination result, caches the eliminated factor */ - template - ISAM2Clique(const RESULT& result) : Base(result), cachedFactor_(result.second) {} + /** Access the gradient contribution */ + const Vector& gradientContribution() const { return gradientContribution_; } /** Construct from a conditional */ - ISAM2Clique(const typename Base::sharedConditional& conditional) : Base(conditional) {} + ISAM2Clique(const sharedConditional& conditional) : Base(conditional) { + throw runtime_error("ISAM2Clique should always be constructed with the elimination result constructor"); } - /** Create from an elimination result, overrides BayesTreeClique::Create(const RESULT&) to cache the eliminated factor */ - template - static shared_ptr Create(const RESULT& result) { return shared_ptr(new This(result)); } - - static shared_ptr Create(const typename Base::sharedConditional& conditional) { return shared_ptr(new This(conditional)); } - - void permuteWithInverse(const Permutation& inversePermutation) { - Base::conditional_->permuteWithInverse(inversePermutation); - if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); - BOOST_FOREACH(const typename Base::shared_ptr& child, Base::children_) { - shared_ptr _child; -#ifndef NDEBUG // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique - _child = boost::dynamic_pointer_cast(child); -#else - _child = boost::static_pointer_cast(child); -#endif - _child->permuteWithInverse(inversePermutation); - } - Base::assertInvariants(); - } - - bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { - bool changed = Base::conditional_->permuteSeparatorWithInverse(inversePermutation); -#ifndef NDEBUG - if(!changed) { - BOOST_FOREACH(Index& separatorKey, Base::conditional_->parents()) { - assert(separatorKey == inversePermutation[separatorKey]); } - BOOST_FOREACH(const typename Base::shared_ptr& child, Base::children_) { - shared_ptr _child = boost::dynamic_pointer_cast(child); // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique - assert(_child->permuteSeparatorWithInverse(inversePermutation) == false); } - } -#endif - if(changed) { - if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); - BOOST_FOREACH(const typename Base::shared_ptr& child, Base::children_) { - shared_ptr _child; - #ifndef NDEBUG // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique - _child = boost::dynamic_pointer_cast(child); - #else - _child = boost::static_pointer_cast(child); - #endif - (void)_child->permuteSeparatorWithInverse(inversePermutation); - } - } - Base::assertInvariants(); - return changed; + /** Construct from an elimination result */ + ISAM2Clique(const std::pair >& result) : + Base(result.first), cachedFactor_(result.second), gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) { + // Compute gradient contribution + const ConditionalType& conditional(*result.first); + gradient << -(conditional.get_R() * conditional.permutation().transpose()).transpose * conditional.get_d(), + -conditional.get_S() * conditional.get_d(); } private: @@ -180,6 +141,7 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(cachedFactor_); + ar & BOOST_SERIALIZATION_NVP(gradientContribution_); } }; From 3b139cbae2d23f93b49c75574aca2726baee7f73 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 12 Dec 2011 16:03:52 +0000 Subject: [PATCH 06/17] (in branch) Merged from trunk r7960-r8057 --- .cproject | 67 +++ configure.ac | 111 +++-- examples/Data/calib.txt | 2 +- examples/Data/measurementsISAM.txt | 12 - examples/Data/posesISAM.txt | 12 - examples/Data/ttpy10.pose | 8 +- examples/Data/ttpy100.pose | 8 +- examples/Data/ttpy20.pose | 8 +- examples/Data/ttpy30.pose | 8 +- examples/Data/ttpy40.pose | 8 +- examples/Data/ttpy50.pose | 8 +- examples/Data/ttpy60.pose | 8 +- examples/Data/ttpy70.pose | 8 +- examples/Data/ttpy80.pose | 8 +- examples/Data/ttpy90.pose | 8 +- examples/vSLAMexample/vISAMexample.cpp | 24 +- examples/vSLAMexample/vSLAMutils.cpp | 49 +-- examples/vSLAMexample/vSLAMutils.h | 4 +- gtsam-broken.h | 182 -------- gtsam.h | 399 +++++++++++++++-- gtsam/base/TestableAssertions.h | 12 + gtsam/base/cholesky.cpp | 17 +- gtsam/geometry/Pose2.h | 10 - gtsam/geometry/Pose3.cpp | 31 +- gtsam/geometry/Pose3.h | 10 +- gtsam/geometry/Rot3M.cpp | 404 +++++++++--------- gtsam/geometry/Rot3M.h | 12 +- gtsam/geometry/tests/testPose3.cpp | 92 +--- gtsam/inference/BayesTree.h | 5 + gtsam/inference/BayesTreeCliqueBase.h | 14 + gtsam/inference/Permutation.h | 7 + gtsam/linear/GaussianFactor.h | 3 + gtsam/linear/HessianFactor.h | 6 + gtsam/linear/JacobianFactor.h | 6 + gtsam/nonlinear/GaussianISAM2.h | 6 + gtsam/nonlinear/ISAM2.h | 23 + gtsam/slam/Landmark2.h | 27 -- gtsam/slam/Makefile.am | 20 +- gtsam/slam/PlanarSLAMGraph.h | 28 -- gtsam/slam/PlanarSLAMOdometry.h | 28 -- gtsam/slam/PlanarSLAMValues.h | 27 -- gtsam/slam/Simulated2DMeasurement.h | 28 -- gtsam/slam/Simulated2DOdometry.h | 28 -- gtsam/slam/Simulated2DOrientedOdometry.h | 28 -- gtsam/slam/Simulated2DOrientedPosePrior.h | 29 -- gtsam/slam/Simulated2DOrientedValues.h | 59 --- gtsam/slam/Simulated2DPointPrior.h | 29 -- gtsam/slam/Simulated2DPosePrior.h | 29 -- gtsam/slam/Simulated2DValues.h | 58 --- gtsam/slam/Simulated3D.cpp | 2 +- gtsam/slam/planarSLAM.h | 19 +- gtsam/slam/simulated2D.h | 16 +- gtsam/slam/simulated2DOriented.h | 30 +- gtsam/slam/simulated3D.cpp | 43 ++ gtsam/slam/simulated3D.h | 132 ++++++ gtsam/slam/tests/testSimulated3D.cpp | 2 +- myconfigure.matlab | 13 +- tests/matlab/test_gtsam.m | 10 + tests/testGaussianISAM2.cpp | 104 +++++ wrap/Argument.cpp | 33 +- wrap/Argument.h | 23 +- wrap/Class.cpp | 100 ++++- wrap/Class.h | 29 +- wrap/Constructor.cpp | 44 +- wrap/Constructor.h | 20 +- wrap/Makefile.am | 95 +++- wrap/Method.cpp | 79 +--- wrap/Method.h | 30 +- wrap/Module.cpp | 227 +++++++--- wrap/Module.h | 18 +- wrap/README | 6 +- wrap/ReturnValue.cpp | 78 ++++ wrap/ReturnValue.h | 53 +++ wrap/StaticMethod.cpp | 100 +++++ wrap/StaticMethod.h | 56 +++ wrap/geometry.h | 39 -- wrap/matlab.h | 4 +- wrap/pop_actor.h | 59 +++ wrap/tests/expected/@Point2/dim.cpp | 3 +- wrap/tests/expected/@Point2/dim.m | 1 - .../expected/@Point2/vectorConfusion.cpp | 11 + wrap/tests/expected/@Point2/vectorConfusion.m | 4 + wrap/tests/expected/@Point2/x.cpp | 3 +- wrap/tests/expected/@Point2/x.m | 1 - wrap/tests/expected/@Point2/y.cpp | 3 +- wrap/tests/expected/@Point2/y.m | 1 - wrap/tests/expected/@Point3/norm.cpp | 3 +- wrap/tests/expected/@Test/Test.m | 1 + .../expected/@Test/arg_EigenConstRef.cpp | 11 + wrap/tests/expected/@Test/arg_EigenConstRef.m | 4 + .../tests/expected/@Test/create_MixedPtrs.cpp | 12 + wrap/tests/expected/@Test/create_MixedPtrs.m | 4 + wrap/tests/expected/@Test/create_ptrs.cpp | 5 +- wrap/tests/expected/@Test/create_ptrs.m | 1 - wrap/tests/expected/@Test/print.cpp | 5 +- .../tests/expected/@Test/return_Point2Ptr.cpp | 12 + wrap/tests/expected/@Test/return_Point2Ptr.m | 4 + wrap/tests/expected/@Test/return_Test.cpp | 12 + wrap/tests/expected/@Test/return_Test.m | 4 + wrap/tests/expected/@Test/return_TestPtr.cpp | 5 +- wrap/tests/expected/@Test/return_TestPtr.m | 1 - wrap/tests/expected/@Test/return_bool.cpp | 5 +- wrap/tests/expected/@Test/return_bool.m | 1 - wrap/tests/expected/@Test/return_double.cpp | 5 +- wrap/tests/expected/@Test/return_double.m | 1 - wrap/tests/expected/@Test/return_field.cpp | 5 +- wrap/tests/expected/@Test/return_field.m | 1 - wrap/tests/expected/@Test/return_int.cpp | 5 +- wrap/tests/expected/@Test/return_int.m | 1 - wrap/tests/expected/@Test/return_matrix1.cpp | 5 +- wrap/tests/expected/@Test/return_matrix1.m | 1 - wrap/tests/expected/@Test/return_matrix2.cpp | 5 +- wrap/tests/expected/@Test/return_matrix2.m | 1 - wrap/tests/expected/@Test/return_pair.cpp | 5 +- wrap/tests/expected/@Test/return_pair.m | 1 - wrap/tests/expected/@Test/return_ptrs.cpp | 5 +- wrap/tests/expected/@Test/return_ptrs.m | 1 - wrap/tests/expected/@Test/return_size_t.cpp | 5 +- wrap/tests/expected/@Test/return_size_t.m | 1 - wrap/tests/expected/@Test/return_string.cpp | 5 +- wrap/tests/expected/@Test/return_string.m | 1 - wrap/tests/expected/@Test/return_vector1.cpp | 5 +- wrap/tests/expected/@Test/return_vector1.m | 1 - wrap/tests/expected/@Test/return_vector2.cpp | 5 +- wrap/tests/expected/@Test/return_vector2.m | 1 - wrap/tests/expected/Makefile | 91 ++++ .../expected/Point3_StaticFunctionRet.cpp | 11 + .../tests/expected/Point3_StaticFunctionRet.m | 4 + wrap/tests/expected/Point3_staticFunction.cpp | 10 + wrap/tests/expected/Point3_staticFunction.m | 4 + wrap/tests/expected/make_geometry.m | 12 +- wrap/tests/expected/new_Point2_.cpp | 3 +- wrap/tests/expected/new_Point2_.m | 2 +- wrap/tests/expected/new_Point2_dd.cpp | 3 +- wrap/tests/expected/new_Point2_dd.m | 2 +- wrap/tests/expected/new_Point3_ddd.cpp | 3 +- wrap/tests/expected/new_Point3_ddd.m | 2 +- wrap/tests/expected/new_Test_.cpp | 5 +- wrap/tests/expected/new_Test_.m | 2 +- wrap/tests/expected/new_Test_b.cpp | 10 + wrap/tests/expected/new_Test_b.m | 4 + wrap/tests/expected/new_Test_dM.cpp | 12 + .../expected_namespaces/@ClassD/ClassD.m | 13 + .../@ns1ClassA/ns1ClassA.m | 13 + .../@ns1ClassB/ns1ClassB.m | 13 + .../@ns2ClassA/memberFunction.cpp | 10 + .../@ns2ClassA/memberFunction.m | 4 + .../@ns2ClassA/ns2ClassA.m | 13 + .../@ns2ClassC/ns2ClassC.m | 13 + .../@ns2ns3ClassB/ns2ns3ClassB.m | 13 + wrap/tests/expected_namespaces/Makefile | 64 +++ .../expected_namespaces/make_testNamespaces.m | 52 +++ .../tests/expected_namespaces/new_ClassD_.cpp | 9 + wrap/tests/expected_namespaces/new_ClassD_.m | 4 + .../expected_namespaces/new_ns1ClassA_.cpp | 9 + .../expected_namespaces/new_ns1ClassA_.m | 4 + .../expected_namespaces/new_ns1ClassB_.cpp | 9 + .../expected_namespaces/new_ns1ClassB_.m | 4 + .../expected_namespaces/new_ns2ClassA_.cpp | 9 + .../expected_namespaces/new_ns2ClassA_.m | 4 + .../expected_namespaces/new_ns2ClassC_.cpp | 9 + .../expected_namespaces/new_ns2ClassC_.m | 4 + .../expected_namespaces/new_ns2ns3ClassB_.cpp | 9 + .../expected_namespaces/new_ns2ns3ClassB_.m | 4 + .../ns2ClassA_afunction.cpp | 9 + .../expected_namespaces/ns2ClassA_afunction.m | 4 + wrap/tests/geometry.h | 80 ++++ wrap/tests/testNamespaces.h | 45 ++ wrap/tests/testSpirit.cpp | 65 ++- wrap/tests/testWrap.cpp | 187 ++++++-- wrap/tests/testWrap1.h | 23 + wrap/utilities.cpp | 49 ++- wrap/utilities.h | 45 +- wrap/wrap.cpp | 17 +- 174 files changed, 3137 insertions(+), 1518 deletions(-) delete mode 100644 examples/Data/measurementsISAM.txt delete mode 100644 examples/Data/posesISAM.txt delete mode 100644 gtsam-broken.h delete mode 100644 gtsam/slam/Landmark2.h delete mode 100644 gtsam/slam/PlanarSLAMGraph.h delete mode 100644 gtsam/slam/PlanarSLAMOdometry.h delete mode 100644 gtsam/slam/PlanarSLAMValues.h delete mode 100644 gtsam/slam/Simulated2DMeasurement.h delete mode 100644 gtsam/slam/Simulated2DOdometry.h delete mode 100644 gtsam/slam/Simulated2DOrientedOdometry.h delete mode 100644 gtsam/slam/Simulated2DOrientedPosePrior.h delete mode 100644 gtsam/slam/Simulated2DOrientedValues.h delete mode 100644 gtsam/slam/Simulated2DPointPrior.h delete mode 100644 gtsam/slam/Simulated2DPosePrior.h delete mode 100644 gtsam/slam/Simulated2DValues.h create mode 100644 gtsam/slam/simulated3D.cpp create mode 100644 gtsam/slam/simulated3D.h create mode 100644 tests/matlab/test_gtsam.m create mode 100644 wrap/ReturnValue.cpp create mode 100644 wrap/ReturnValue.h create mode 100644 wrap/StaticMethod.cpp create mode 100644 wrap/StaticMethod.h delete mode 100644 wrap/geometry.h create mode 100644 wrap/pop_actor.h create mode 100644 wrap/tests/expected/@Point2/vectorConfusion.cpp create mode 100644 wrap/tests/expected/@Point2/vectorConfusion.m create mode 100644 wrap/tests/expected/@Test/arg_EigenConstRef.cpp create mode 100644 wrap/tests/expected/@Test/arg_EigenConstRef.m create mode 100644 wrap/tests/expected/@Test/create_MixedPtrs.cpp create mode 100644 wrap/tests/expected/@Test/create_MixedPtrs.m create mode 100644 wrap/tests/expected/@Test/return_Point2Ptr.cpp create mode 100644 wrap/tests/expected/@Test/return_Point2Ptr.m create mode 100644 wrap/tests/expected/@Test/return_Test.cpp create mode 100644 wrap/tests/expected/@Test/return_Test.m create mode 100644 wrap/tests/expected/Makefile create mode 100644 wrap/tests/expected/Point3_StaticFunctionRet.cpp create mode 100644 wrap/tests/expected/Point3_StaticFunctionRet.m create mode 100644 wrap/tests/expected/Point3_staticFunction.cpp create mode 100644 wrap/tests/expected/Point3_staticFunction.m create mode 100644 wrap/tests/expected/new_Test_b.cpp create mode 100644 wrap/tests/expected/new_Test_b.m create mode 100644 wrap/tests/expected/new_Test_dM.cpp create mode 100644 wrap/tests/expected_namespaces/@ClassD/ClassD.m create mode 100644 wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m create mode 100644 wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m create mode 100644 wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp create mode 100644 wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.m create mode 100644 wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m create mode 100644 wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m create mode 100644 wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m create mode 100644 wrap/tests/expected_namespaces/Makefile create mode 100644 wrap/tests/expected_namespaces/make_testNamespaces.m create mode 100644 wrap/tests/expected_namespaces/new_ClassD_.cpp create mode 100644 wrap/tests/expected_namespaces/new_ClassD_.m create mode 100644 wrap/tests/expected_namespaces/new_ns1ClassA_.cpp create mode 100644 wrap/tests/expected_namespaces/new_ns1ClassA_.m create mode 100644 wrap/tests/expected_namespaces/new_ns1ClassB_.cpp create mode 100644 wrap/tests/expected_namespaces/new_ns1ClassB_.m create mode 100644 wrap/tests/expected_namespaces/new_ns2ClassA_.cpp create mode 100644 wrap/tests/expected_namespaces/new_ns2ClassA_.m create mode 100644 wrap/tests/expected_namespaces/new_ns2ClassC_.cpp create mode 100644 wrap/tests/expected_namespaces/new_ns2ClassC_.m create mode 100644 wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp create mode 100644 wrap/tests/expected_namespaces/new_ns2ns3ClassB_.m create mode 100644 wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp create mode 100644 wrap/tests/expected_namespaces/ns2ClassA_afunction.m create mode 100644 wrap/tests/geometry.h create mode 100644 wrap/tests/testNamespaces.h create mode 100644 wrap/tests/testWrap1.h diff --git a/.cproject b/.cproject index a35bd2fed..35f7946a4 100644 --- a/.cproject +++ b/.cproject @@ -36,6 +36,9 @@ @@ -681,6 +684,22 @@ true true + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + make -j2 @@ -2029,6 +2048,22 @@ true true + + make + -j5 + check + true + false + true + + + make + -j5 + install + true + false + true + make -j2 @@ -2045,6 +2080,38 @@ true true + + make + -j2 + tests/testSpirit.run + true + true + true + + + make + -j2 + tests/testWrap.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + diff --git a/configure.ac b/configure.ac index 6b057ba46..5929b22ee 100644 --- a/configure.ac +++ b/configure.ac @@ -30,8 +30,11 @@ DX_PS_FEATURE(OFF) DX_INIT_DOXYGEN(gtsam) # Check for OS -AC_CANONICAL_HOST # needs to be called at some point earlier -AM_CONDITIONAL([DARWIN], [case $host_os in darwin*) true;; *) false;; esac]) +# needs to be called at some point earlier +AC_CANONICAL_HOST +AM_CONDITIONAL([DARWIN], [case $host_os in darwin*) true;; *) false;; esac]) +AM_CONDITIONAL([LINUX], [case $host_os in linux*) true;; *) false;; esac]) +AM_CONDITIONAL([IS_64BIT], [case $host_cpu in *x86_64*) true;; *) false;; esac]) # enable debug variable AC_ARG_ENABLE([debug], @@ -44,20 +47,6 @@ AC_ARG_ENABLE([debug], AM_CONDITIONAL([DEBUG], [test x$debug = xtrue]) - -AC_CANONICAL_HOST -# We need to determine what os we are on to determine if we need to do -# special things because we are on a mac -case $host_os in - darwin* ) - # Do something specific for mac - ISMAC=true - ;; - *) - ISMAC=false - ;; -esac - # enable profiling AC_ARG_ENABLE([profiling], [ --enable-profiling Enable profiling], @@ -91,6 +80,77 @@ AC_ARG_ENABLE([install_cppunitlite], AM_CONDITIONAL([ENABLE_INSTALL_CPPUNITLITE], [test x$install_cppunitlite = xtrue]) +# enable matlab toolbox generation +AC_ARG_ENABLE([build_toolbox], + [ --enable-build-toolbox Enable building of the Matlab toolbox], + [case "${enableval}" in + yes) build_toolbox=true ;; + no) build_toolbox=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-build-toolbox]) ;; + esac],[build_toolbox=false]) + +AM_CONDITIONAL([ENABLE_BUILD_TOOLBOX], [test x$build_toolbox = xtrue]) + +# enable installation of matlab tests +AC_ARG_ENABLE([install_matlab_tests], + [ --enable-install-matlab-tests Enable installation of tests for the Matlab toolbox], + [case "${enableval}" in + yes) install_matlab_tests=true ;; + no) install_matlab_tests=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-matlab-tests]) ;; + esac],[install_matlab_tests=true]) + +AM_CONDITIONAL([ENABLE_INSTALL_MATLAB_TESTS], [test x$install_matlab_tests = xtrue]) + +# enable installation of matlab examples +AC_ARG_ENABLE([install_matlab_examples], + [ --enable-install-matlab-examples Enable installation of examples for the Matlab toolbox], + [case "${enableval}" in + yes) install_matlab_examples=true ;; + no) install_matlab_examples=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-matlab-examples]) ;; + esac],[install_matlab_examples=true]) + +AM_CONDITIONAL([ENABLE_INSTALL_MATLAB_EXAMPLES], [test x$install_matlab_examples = xtrue]) + +# Matlab toolbox: optional flag to change location of toolbox, defaults to install prefix +AC_ARG_WITH([toolbox], + [AS_HELP_STRING([--with-toolbox], + [specify the matlab toolbox directory for installation])], + [toolbox=$withval], + [toolbox=$prefix]) +AC_SUBST([toolbox]) + +# enable installation of the wrap utility +AC_ARG_ENABLE([install_wrap], + [ --enable-install-wrap Enable installation of the wrap tool for generating matlab interfaces], + [case "${enableval}" in + yes) install_wrap=true ;; + no) install_wrap=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-wrap]) ;; + esac],[install_wrap=false]) + +AM_CONDITIONAL([ENABLE_INSTALL_WRAP], [test x$install_wrap = xtrue]) + +# enable unsafe mode for wrap +AC_ARG_ENABLE([unsafe_wrap], + [ --enable-unsafe-wrap Enable using unsafe mode in wrap], + [case "${enableval}" in + yes) unsafe_wrap=true ;; + no) unsafe_wrap=false ;; + *) AC_MSG_ERROR([bad value ${enableval} for --enable-unsafe-wrap]) ;; + esac],[unsafe_wrap=false]) + +AM_CONDITIONAL([ENABLE_UNSAFE_WRAP], [test x$unsafe_wrap = xtrue]) + +# wrap install path: optional flag to change location of wrap, defaults to install prefix/bin +AC_ARG_WITH([wrap], + [AS_HELP_STRING([--with-wrap], + [specify the wrap directory for installation])], + [wrap=$withval], + [wrap=$prefix/bin]) +AC_SUBST([wrap]) + # Checks for programs. AC_PROG_CXX AC_PROG_CC @@ -115,25 +175,6 @@ AC_CHECK_FUNCS([pow sqrt]) # Check for boost AX_BOOST_BASE([1.40]) -# enable matlab toolbox generation -AC_ARG_ENABLE([build_toolbox], - [ --enable-build-toolbox Enable building of the Matlab toolbox], - [case "${enableval}" in - yes) build_toolbox=true ;; - no) build_toolbox=false ;; - *) AC_MSG_ERROR([bad value ${enableval} for --enable-build-toolbox]) ;; - esac],[build_toolbox=false]) - -AM_CONDITIONAL([ENABLE_BUILD_TOOLBOX], [test x$build_toolbox = xtrue]) - -# Matlab toolbox: optional flag to change location of toolbox, defaults to install prefix -AC_ARG_WITH([toolbox], - [AS_HELP_STRING([--with-toolbox], - [specify the matlab toolbox directory for installation])], - [toolbox=$withval], - [toolbox=$prefix]) -AC_SUBST([toolbox]) - AC_CONFIG_FILES([CppUnitLite/Makefile \ wrap/Makefile \ gtsam/3rdparty/Makefile \ diff --git a/examples/Data/calib.txt b/examples/Data/calib.txt index 8eeb7e656..cb8b77384 100644 --- a/examples/Data/calib.txt +++ b/examples/Data/calib.txt @@ -1 +1 @@ -800 600 -1119.61507797 1119.61507797 399.5 299.5 +800 600 1119.61507797 1119.61507797 399.5 299.5 diff --git a/examples/Data/measurementsISAM.txt b/examples/Data/measurementsISAM.txt deleted file mode 100644 index 7325c7dd9..000000000 --- a/examples/Data/measurementsISAM.txt +++ /dev/null @@ -1,12 +0,0 @@ -10 -ttpy10.feat -ttpy20.feat -ttpy30.feat -ttpy40.feat -ttpy50.feat -ttpy60.feat -ttpy70.feat -ttpy80.feat -ttpy90.feat -ttpy100.feat - diff --git a/examples/Data/posesISAM.txt b/examples/Data/posesISAM.txt deleted file mode 100644 index ebe39c6f2..000000000 --- a/examples/Data/posesISAM.txt +++ /dev/null @@ -1,12 +0,0 @@ -10 -ttpy10.pose -ttpy20.pose -ttpy30.pose -ttpy40.pose -ttpy50.pose -ttpy60.pose -ttpy70.pose -ttpy80.pose -ttpy90.pose -ttpy100.pose - diff --git a/examples/Data/ttpy10.pose b/examples/Data/ttpy10.pose index ee0f6231a..b9226c723 100644 --- a/examples/Data/ttpy10.pose +++ b/examples/Data/ttpy10.pose @@ -1,4 +1,4 @@ -0.939693 0.34202 0 0 --0.241845 0.664463 -0.707107 0 --0.241845 0.664463 0.707107 0 -34.202 -93.9693 100 1 +0.93969 0.24185 -0.24185 34.202 +0.34202 -0.66446 0.66446 -93.969 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy100.pose b/examples/Data/ttpy100.pose index 4225e3d4e..cbd21233f 100644 --- a/examples/Data/ttpy100.pose +++ b/examples/Data/ttpy100.pose @@ -1,4 +1,4 @@ --0.939693 -0.34202 0 0 -0.241845 -0.664463 -0.707107 0 -0.241845 -0.664463 0.707107 0 --34.202 93.9693 100 1 +-0.93969 -0.24185 0.24185 -34.202 +-0.34202 0.66446 -0.66446 93.969 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy20.pose b/examples/Data/ttpy20.pose index f8b252855..bba2606d0 100644 --- a/examples/Data/ttpy20.pose +++ b/examples/Data/ttpy20.pose @@ -1,4 +1,4 @@ -0.766044 0.642788 0 0 --0.454519 0.541675 -0.707107 0 --0.454519 0.541675 0.707107 0 -64.2788 -76.6044 100 1 +0.76604 0.45452 -0.45452 64.279 +0.64279 -0.54168 0.54168 -76.604 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy30.pose b/examples/Data/ttpy30.pose index f7ad9f674..737e391f2 100644 --- a/examples/Data/ttpy30.pose +++ b/examples/Data/ttpy30.pose @@ -1,4 +1,4 @@ -0.5 0.866025 0 0 --0.612372 0.353553 -0.707107 0 --0.612372 0.353553 0.707107 0 -86.6025 -50 100 1 +0.5 0.61237 -0.61237 86.603 +0.86603 -0.35355 0.35355 -50 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy40.pose b/examples/Data/ttpy40.pose index e9c9cd251..4e241c130 100644 --- a/examples/Data/ttpy40.pose +++ b/examples/Data/ttpy40.pose @@ -1,4 +1,4 @@ -0.173648 0.984808 0 0 --0.696364 0.122788 -0.707107 0 --0.696364 0.122788 0.707107 0 -98.4808 -17.3648 100 1 +0.17365 0.69636 -0.69636 98.481 +0.98481 -0.12279 0.12279 -17.365 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy50.pose b/examples/Data/ttpy50.pose index 9c84b54f3..5a57b10ef 100644 --- a/examples/Data/ttpy50.pose +++ b/examples/Data/ttpy50.pose @@ -1,4 +1,4 @@ --0.173648 0.984808 0 0 --0.696364 -0.122788 -0.707107 0 --0.696364 -0.122788 0.707107 0 -98.4808 17.3648 100 1 +-0.17365 0.69636 -0.69636 98.481 +0.98481 0.12279 -0.12279 17.365 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy60.pose b/examples/Data/ttpy60.pose index 9c0a8c942..4e5455a0d 100644 --- a/examples/Data/ttpy60.pose +++ b/examples/Data/ttpy60.pose @@ -1,4 +1,4 @@ --0.5 0.866025 0 0 --0.612372 -0.353553 -0.707107 0 --0.612372 -0.353553 0.707107 0 -86.6025 50 100 1 +-0.5 0.61237 -0.61237 86.603 +0.86603 0.35355 -0.35355 50 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy70.pose b/examples/Data/ttpy70.pose index 0c2254de6..608e9ff7c 100644 --- a/examples/Data/ttpy70.pose +++ b/examples/Data/ttpy70.pose @@ -1,4 +1,4 @@ --0.766044 0.642788 0 0 --0.454519 -0.541675 -0.707107 0 --0.454519 -0.541675 0.707107 0 -64.2788 76.6044 100 1 +-0.76604 0.45452 -0.45452 64.279 +0.64279 0.54168 -0.54168 76.604 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy80.pose b/examples/Data/ttpy80.pose index 9d6491304..8a6ae4cd1 100644 --- a/examples/Data/ttpy80.pose +++ b/examples/Data/ttpy80.pose @@ -1,4 +1,4 @@ --0.939693 0.34202 0 0 --0.241845 -0.664463 -0.707107 0 --0.241845 -0.664463 0.707107 0 -34.202 93.9693 100 1 +-0.93969 0.24185 -0.24185 34.202 +0.34202 0.66446 -0.66446 93.969 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/Data/ttpy90.pose b/examples/Data/ttpy90.pose index b51c1a58f..863f71fea 100644 --- a/examples/Data/ttpy90.pose +++ b/examples/Data/ttpy90.pose @@ -1,4 +1,4 @@ --1 0 0 0 -0 -0.707107 -0.707107 0 -0 -0.707107 0.707107 0 -0 100 100 1 +-1 -0 0 0 +0 0.70711 -0.70711 100 +0 -0.70711 -0.70711 100 +0 0 0 1 diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index ed3a68ba8..bd35e3636 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -17,6 +17,7 @@ */ #include +#include using namespace boost; // Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h @@ -40,8 +41,8 @@ using namespace boost; /* ************************************************************************* */ #define CALIB_FILE "calib.txt" #define LANDMARKS_FILE "landmarks.txt" -#define POSES_FILE "posesISAM.txt" -#define MEASUREMENTS_FILE "measurementsISAM.txt" +#define POSES_FILE "poses.txt" +#define MEASUREMENTS_FILE "measurements.txt" // Base data folder string g_dataFolder; @@ -49,8 +50,8 @@ string g_dataFolder; // Store groundtruth values, read from files shared_ptrK g_calib; map g_landmarks; // map: -std::vector g_poses; // prior camera poses at each frame -std::vector > g_measurements; // feature sets detected at each frame +map g_poses; // map: +std::map > g_measurements; // feature sets detected at each frame // Noise models SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f)); @@ -69,7 +70,7 @@ void readAllDataISAM() { g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE); // Read groundtruth camera poses. These will be used later as intial estimates for pose nodes. - g_poses = readPosesISAM(g_dataFolder, POSES_FILE); + g_poses = readPoses(g_dataFolder, POSES_FILE); // Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark. g_measurements = readAllMeasurementsISAM(g_dataFolder, MEASUREMENTS_FILE); @@ -80,7 +81,7 @@ void readAllDataISAM() { * Setup newFactors and initialValues for each new pose and set of measurements at each frame. */ void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, - int pose_id, Pose3& pose, std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { + int pose_id, const Pose3& pose, const std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { // Create a graph of newFactors with new measurements newFactors = shared_ptr (new Graph()); @@ -121,16 +122,19 @@ int main(int argc, char* argv[]) { g_dataFolder = string(argv[1]) + "/"; readAllDataISAM(); - // Create an NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates + // Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates int relinearizeInterval = 3; NonlinearISAM isam(relinearizeInterval); - // At each frame i with new camera pose and new set of measurements associated with it, + // At each frame (poseId) with new camera pose and set of associated measurements, // create a graph of new factors and update ISAM - for (size_t i = 0; i < g_measurements.size(); i++) { + typedef std::map > FeatureMap; + BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) { + const int poseId = features.first; shared_ptr newFactors; shared_ptr initialValues; - createNewFactors(newFactors, initialValues, i, g_poses[i], g_measurements[i], measurementSigma, g_calib); + createNewFactors(newFactors, initialValues, poseId, g_poses[poseId], + features.second, measurementSigma, g_calib); isam.update(*newFactors, *initialValues); visualSLAM::Values currentEstimate = isam.estimate(); diff --git a/examples/vSLAMexample/vSLAMutils.cpp b/examples/vSLAMexample/vSLAMutils.cpp index ae9042f2a..1c3912f2d 100644 --- a/examples/vSLAMexample/vSLAMutils.cpp +++ b/examples/vSLAMexample/vSLAMutils.cpp @@ -46,10 +46,6 @@ std::map readLandMarks(const std::string& landmarkFile) { } /* ************************************************************************* */ -/** - * Read pose from file, output by Panda3D. - * Warning: row major!!! - */ gtsam::Pose3 readPose(const char* Fn) { ifstream poseFile(Fn); if (!poseFile) { @@ -62,18 +58,7 @@ gtsam::Pose3 readPose(const char* Fn) { poseFile >> v[i]; poseFile.close(); - // Because panda3d's camera is z-up, y-view, - // we swap z and y to have y-up, z-view, then negate z to stick with the right-hand rule - //... similar to OpenGL's camera - for (int i = 0; i<3; i++) { - float t = v[4+i]; - v[4+i] = v[8+i]; - v[8+i] = -t; - } - - ::Vector vec = Vector_(16, v); - - Matrix T = Matrix_(4,4, vec); // column order !!! + Matrix T = Matrix_(4,4, v); // row order !!! Pose3 pose(T); return pose; @@ -166,28 +151,7 @@ std::vector readAllMeasurements(const std::string& baseFolder, const } /* ************************************************************************* */ -std::vector readPosesISAM(const std::string& baseFolder, const std::string& posesFn) { - ifstream posesFile((baseFolder+posesFn).c_str()); - if (!posesFile) { - cout << "Cannot read all pose ISAM file: " << posesFn << endl; - exit(0); - } - int numPoses; - posesFile >> numPoses; - vector poses; - for (int i = 0; i> poseFileName; - - Pose3 pose = readPose((baseFolder+poseFileName).c_str()); - poses.push_back(pose); - } - - return poses; -} - -/* ************************************************************************* */ -std::vector > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) { +std::map > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) { ifstream measurementsFile((baseFolder+measurementsFn).c_str()); if (!measurementsFile) { cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl; @@ -196,13 +160,16 @@ std::vector > readAllMeasurementsISAM(const std::string& int numPoses; measurementsFile >> numPoses; - std::vector > allFeatures; + std::map > allFeatures; for (int i = 0; i> poseId; + string featureFileName; measurementsFile >> featureFileName; - vector features = readFeatures(-1, (baseFolder+featureFileName).c_str()); // we don't care about pose id in ISAM - allFeatures.push_back(features); + vector features = readFeatures(poseId, (baseFolder+featureFileName).c_str()); + allFeatures[poseId] = features; } return allFeatures; diff --git a/examples/vSLAMexample/vSLAMutils.h b/examples/vSLAMexample/vSLAMutils.h index f1483cd70..52d492224 100644 --- a/examples/vSLAMexample/vSLAMutils.h +++ b/examples/vSLAMexample/vSLAMutils.h @@ -29,11 +29,9 @@ std::map readLandMarks(const std::string& landmarkFile); gtsam::Pose3 readPose(const char* poseFn); std::map readPoses(const std::string& baseFolder, const std::string& posesFN); -std::vector readPosesISAM(const std::string& baseFolder, const std::string& posesFN); - gtsam::shared_ptrK readCalibData(const std::string& calibFn); std::vector readFeatureFile(const char* filename); std::vector readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn); -std::vector< std::vector > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn); +std::map > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn); diff --git a/gtsam-broken.h b/gtsam-broken.h deleted file mode 100644 index 09113a695..000000000 --- a/gtsam-broken.h +++ /dev/null @@ -1,182 +0,0 @@ -// These are considered to be broken and will be added back as they start working -// It's assumed that there have been interface changes that might break this - -class Ordering{ - Ordering(string key); - void print(string s) const; - bool equals(const Ordering& ord, double tol) const; - Ordering subtract(const Ordering& keys) const; - void unique (); - void reverse (); - void push_back(string s); -}; - -class GaussianFactorSet { - GaussianFactorSet(); - void push_back(GaussianFactor* factor); -}; - -class Simulated2DValues { - Simulated2DValues(); - void print(string s) const; - void insertPose(int i, const Point2& p); - void insertPoint(int j, const Point2& p); - int nrPoses() const; - int nrPoints() const; - Point2* pose(int i); - Point2* point(int j); -}; - -class Simulated2DOrientedValues { - Simulated2DOrientedValues(); - void print(string s) const; - void insertPose(int i, const Pose2& p); - void insertPoint(int j, const Point2& p); - int nrPoses() const; - int nrPoints() const; - Pose2* pose(int i); - Point2* point(int j); -}; - -class Simulated2DPosePrior { - Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i); - void print(string s) const; - double error(const Simulated2DValues& c) const; -}; - -class Simulated2DOrientedPosePrior { - Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i); - void print(string s) const; - double error(const Simulated2DOrientedValues& c) const; -}; - -class Simulated2DPointPrior { - Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i); - void print(string s) const; - double error(const Simulated2DValues& c) const; -}; - -class Simulated2DOdometry { - Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2); - void print(string s) const; - double error(const Simulated2DValues& c) const; -}; - -class Simulated2DOrientedOdometry { - Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2); - void print(string s) const; - double error(const Simulated2DOrientedValues& c) const; -}; - -class Simulated2DMeasurement { - Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j); - void print(string s) const; - double error(const Simulated2DValues& c) const; -}; - -// These are currently broken -// Solve by parsing a namespace pose2SLAM::Values and making a Pose2SLAMValues class -// We also have to solve the shared pointer mess to avoid duplicate methods - -class GaussianFactor { - GaussianFactor(string key1, - Matrix A1, - Vector b_in, - const SharedDiagonal& model); - GaussianFactor(string key1, - Matrix A1, - string key2, - Matrix A2, - Vector b_in, - const SharedDiagonal& model); - GaussianFactor(string key1, - Matrix A1, - string key2, - Matrix A2, - string key3, - Matrix A3, - Vector b_in, - const SharedDiagonal& model); - bool involves(string key) const; - Matrix getA(string key) const; - pair matrix(const Ordering& ordering) const; - pair eliminate(string key) const; -}; - -class GaussianFactorGraph { - GaussianConditional* eliminateOne(string key); - GaussianBayesNet* eliminate_(const Ordering& ordering); - VectorValues* optimize_(const Ordering& ordering); - pair matrix(const Ordering& ordering) const; - VectorValues* steepestDescent_(const VectorValues& x0) const; - VectorValues* conjugateGradientDescent_(const VectorValues& x0) const; -}; - - -class Pose2Values{ - Pose2Values(); - Pose2 get(string key) const; - void insert(string name, const Pose2& val); - void print(string s) const; - void clear(); - int size(); -}; - -class Pose2Factor { - Pose2Factor(string key1, string key2, - const Pose2& measured, Matrix measurement_covariance); - void print(string name) const; - double error(const Pose2Values& c) const; - size_t size() const; - GaussianFactor* linearize(const Pose2Values& config) const; -}; - -class Pose2Graph{ - Pose2Graph(); - void print(string s) const; - GaussianFactorGraph* linearize_(const Pose2Values& config) const; - void push_back(Pose2Factor* factor); -}; - -class SymbolicFactor{ - SymbolicFactor(const Ordering& keys); - void print(string s) const; -}; - -class Simulated2DPosePrior { - GaussianFactor* linearize(const Simulated2DValues& config) const; -}; - -class Simulated2DOrientedPosePrior { - GaussianFactor* linearize(const Simulated2DOrientedValues& config) const; -}; - -class Simulated2DPointPrior { - GaussianFactor* linearize(const Simulated2DValues& config) const; -}; - -class Simulated2DOdometry { - GaussianFactor* linearize(const Simulated2DValues& config) const; -}; - -class Simulated2DOrientedOdometry { - GaussianFactor* linearize(const Simulated2DOrientedValues& config) const; -}; - -class Simulated2DMeasurement { - GaussianFactor* linearize(const Simulated2DValues& config) const; -}; - -class Pose2SLAMOptimizer { - Pose2SLAMOptimizer(string dataset_name); - void print(string s) const; - void update(Vector x) const; - Vector optimize() const; - double error() const; - Matrix a1() const; - Matrix a2() const; - Vector b1() const; - Vector b2() const; -}; - - diff --git a/gtsam.h b/gtsam.h index 331bdd09a..071ababc8 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1,29 +1,139 @@ +/** + * GTSAM Wrap Module Definition + * + * These are the current classes available through the matlab toolbox interface, + * add more functions/classes as they are available. + * + * Requirements: + * Classes must start with an uppercase letter + * Only one Method/Constructor per line + * Methods can return + * - Eigen types: Matrix, Vector + * - C/C++ basic types: string, bool, size_t, int, double + * - void + * - Any class with which be copied with boost::make_shared() + * - boost::shared_ptr of any object type + * Limitations on methods + * - Parsing does not support overloading + * - There can only be one method with a given name + * Arguments to functions any of + * - Eigen types: Matrix, Vector + * - Eigen types and classes as an optionally const reference + * - C/C++ basic types: string, bool, size_t, int, double + * - Any class with which be copied with boost::make_shared() (except Eigen) + * - boost::shared_ptr of any object type (except Eigen) + * Comments can use either C++ or C style, with multiple lines + * Namespace definitions + * - Names of namespaces must start with a lowercase letter + * - start a namespace with "namespace {" + * - end a namespace with exactly "}///\namespace [namespace_name]", optionally adding the name of the namespace + * - This ending is not C++ standard, and must contain "}///\namespace" to parse + * - Namespaces can be nested + * Namespace usage + * - Namespaces can be specified for classes in arguments and return values + * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" + * Methods must start with a lowercase letter + * Static methods must start with a letter (upper or lowercase) and use the "static" keyword + * Includes in C++ wrappers + * - By default, the include will be <[classname].h> + * - To override, add a full include statement inside the class definition + */ + +/** + * Status: + * - TODO: global functions + * - TODO: default values for arguments + * - TODO: overloaded functions + * - TODO: Handle Rot3M conversions to quaternions + */ + +// Everything is in the gtsam namespace, so we avoid copying everything in +using namespace gtsam; + class Point2 { Point2(); Point2(double x, double y); + static Point2 Expmap(Vector v); + static Vector Logmap(const Point2& p); void print(string s) const; double x(); double y(); + Vector localCoordinates(const Point2& p); + Point2 compose(const Point2& p2); + Point2 between(const Point2& p2); + Point2 retract(Vector v); }; class Point3 { Point3(); Point3(double x, double y, double z); Point3(Vector v); + static Point3 Expmap(Vector v); + static Vector Logmap(const Point3& p); void print(string s) const; + bool equals(const Point3& p, double tol); Vector vector() const; double x(); double y(); double z(); + Vector localCoordinates(const Point3& p); + Point3 retract(Vector v); + Point3 compose(const Point3& p2); + Point3 between(const Point3& p2); }; class Rot2 { Rot2(); Rot2(double theta); + static Rot2 Expmap(Vector v); + static Vector Logmap(const Rot2& p); + static Rot2 fromAngle(double theta); + static Rot2 fromDegrees(double theta); + static Rot2 fromCosSin(double c, double s); + static Rot2 relativeBearing(const Point2& d); // Ignoring derivative + static Rot2 atan2(double y, double x); void print(string s) const; - bool equals(const Rot2& pose, double tol) const; + bool equals(const Rot2& rot, double tol) const; + double theta() const; + double degrees() const; double c() const; double s() const; + Vector localCoordinates(const Rot2& p); + Rot2 retract(Vector v); + Rot2 compose(const Rot2& p2); + Rot2 between(const Rot2& p2); +}; + +class Rot3 { + Rot3(); + Rot3(Matrix R); + static Rot3 Expmap(Vector v); + static Vector Logmap(const Rot3& p); + static Rot3 ypr(double y, double p, double r); + static Rot3 Rx(double t); + static Rot3 Ry(double t); + static Rot3 Rz(double t); + static Rot3 RzRyRx(double x, double y, double z); + static Rot3 RzRyRx(const Vector& xyz); + static Rot3 yaw (double t); // positive yaw is to right (as in aircraft heading) + static Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) + static Rot3 roll (double t); // positive roll is to right (increasing yaw in aircraft) + static Rot3 quaternion(double w, double x, double y, double z); + static Rot3 rodriguez(const Vector& v); + Matrix matrix() const; + Matrix transpose() const; + Vector xyz() const; + Vector ypr() const; + double roll() const; + double pitch() const; + double yaw() const; +// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly + void print(string s) const; + bool equals(const Rot3& rot, double tol) const; + Vector localCoordinates(const Rot3& p); + Rot3 retract(Vector v); + Rot3 compose(const Rot3& p2); + Rot3 between(const Rot3& p2); }; class Pose2 { @@ -32,15 +142,41 @@ class Pose2 { Pose2(double theta, const Point2& t); Pose2(const Rot2& r, const Point2& t); Pose2(Vector v); + static Pose2 Expmap(Vector v); + static Vector Logmap(const Pose2& p); void print(string s) const; bool equals(const Pose2& pose, double tol) const; double x() const; double y() const; double theta() const; int dim() const; - Pose2* compose_(const Pose2& p2); - Pose2* between_(const Pose2& p2); Vector localCoordinates(const Pose2& p); + Pose2 retract(Vector v); + Pose2 compose(const Pose2& p2); + Pose2 between(const Pose2& p2); + Rot2 bearing(const Point2& point); + double range(const Point2& point); +}; + +class Pose3 { + Pose3(); + Pose3(const Rot3& r, const Point3& t); + Pose3(Vector v); + Pose3(Matrix t); + static Pose3 Expmap(Vector v); + static Vector Logmap(const Pose3& p); + void print(string s) const; + bool equals(const Pose3& pose, double tol) const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + Matrix adjointMap() const; + Pose3 compose(const Pose3& p2); + Pose3 between(const Pose3& p2); + Pose3 retract(Vector v); + Point3 translation() const; + Rot3 rotation() const; }; class SharedGaussian { @@ -54,6 +190,12 @@ class SharedDiagonal { Vector sample() const; }; +class SharedNoiseModel { +#include + SharedNoiseModel(const SharedDiagonal& model); + SharedNoiseModel(const SharedGaussian& model); +}; + class VectorValues { VectorValues(); VectorValues(int nVars, int varDim); @@ -118,6 +260,14 @@ class GaussianFactorGraph { Matrix sparseJacobian_() const; }; +class GaussianSequentialSolver { + GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); + GaussianBayesNet* eliminate() const; + VectorValues* optimize() const; + GaussianFactor* marginalFactor(int j) const; + Matrix marginalCovariance(int j) const; +}; + class KalmanFilter { KalmanFilter(Vector x, const SharedDiagonal& model); void print(string s) const; @@ -129,14 +279,6 @@ class KalmanFilter { void update(Matrix H, Vector z, const SharedDiagonal& model); }; -class Landmark2 { - Landmark2(); - Landmark2(double x, double y); - void print(string s) const; - double x(); - double y(); -}; - class Ordering { Ordering(); void print(string s) const; @@ -144,22 +286,28 @@ class Ordering { void push_back(string key); }; -class PlanarSLAMValues { - PlanarSLAMValues(); +// Planar SLAM example domain +namespace planarSLAM { + +class Values { +#include + Values(); void print(string s) const; - Pose2* pose(int key); + Pose2 pose(int key) const; + Point2 point(int key) const; void insertPose(int key, const Pose2& pose); void insertPoint(int key, const Point2& point); }; -class PlanarSLAMGraph { - PlanarSLAMGraph(); +class Graph { +#include + Graph(); void print(string s) const; - double error(const PlanarSLAMValues& values) const; - Ordering* orderingCOLAMD(const PlanarSLAMValues& values) const; - GaussianFactorGraph* linearize(const PlanarSLAMValues& values, + double error(const planarSLAM::Values& values) const; + Ordering* orderingCOLAMD(const planarSLAM::Values& values) const; + GaussianFactorGraph* linearize(const planarSLAM::Values& values, const Ordering& ordering) const; void addPrior(int key, const Pose2& pose, const SharedNoiseModel& noiseModel); @@ -169,18 +317,215 @@ class PlanarSLAMGraph { void addRange(int poseKey, int pointKey, double range, const SharedNoiseModel& noiseModel); void addBearingRange(int poseKey, int pointKey, const Rot2& bearing, double range, const SharedNoiseModel& noiseModel); - PlanarSLAMValues* optimize_(const PlanarSLAMValues& initialEstimate); + planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate); }; -class PlanarSLAMOdometry { - PlanarSLAMOdometry(int key1, int key2, const Pose2& measured, +class Odometry { +#include + Odometry(int key1, int key2, const Pose2& measured, const SharedNoiseModel& model); void print(string s) const; - GaussianFactor* linearize(const PlanarSLAMValues& center, const Ordering& ordering) const; + GaussianFactor* linearize(const planarSLAM::Values& center, const Ordering& ordering) const; }; -class GaussianSequentialSolver { - GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); - GaussianBayesNet* eliminate(); - VectorValues* optimize(); +}///\namespace planarSLAM + +// Simulated2D Example Domain +namespace simulated2D { + +class Values { +#include + Values(); + void insertPose(int i, const Point2& p); + void insertPoint(int j, const Point2& p); + int nrPoses() const; + int nrPoints() const; + Point2 pose(int i); + Point2 point(int j); }; + +class Graph { +#include + Graph(); +}; + +// TODO: add factors, etc. + +}///\namespace simulated2D + +// Simulated2DOriented Example Domain +namespace simulated2DOriented { + +class Values { +#include + Values(); + void insertPose(int i, const Pose2& p); + void insertPoint(int j, const Point2& p); + int nrPoses() const; + int nrPoints() const; + Pose2 pose(int i); + Point2 point(int j); +}; + +class Graph { +#include + Graph(); +}; + +// TODO: add factors, etc. + +}///\namespace simulated2DOriented + +//// These are considered to be broken and will be added back as they start working +//// It's assumed that there have been interface changes that might break this +// +//class Ordering{ +// Ordering(string key); +// void print(string s) const; +// bool equals(const Ordering& ord, double tol) const; +// Ordering subtract(const Ordering& keys) const; +// void unique (); +// void reverse (); +// void push_back(string s); +//}; +// +//class GaussianFactorSet { +// GaussianFactorSet(); +// void push_back(GaussianFactor* factor); +//}; +// +//class Simulated2DPosePrior { +// Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i); +// void print(string s) const; +// double error(const Simulated2DValues& c) const; +//}; +// +//class Simulated2DOrientedPosePrior { +// Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i); +// void print(string s) const; +// double error(const Simulated2DOrientedValues& c) const; +//}; +// +//class Simulated2DPointPrior { +// Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i); +// void print(string s) const; +// double error(const Simulated2DValues& c) const; +//}; +// +//class Simulated2DOdometry { +// Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2); +// void print(string s) const; +// double error(const Simulated2DValues& c) const; +//}; +// +//class Simulated2DOrientedOdometry { +// Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2); +// void print(string s) const; +// double error(const Simulated2DOrientedValues& c) const; +//}; +// +//class Simulated2DMeasurement { +// Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j); +// void print(string s) const; +// double error(const Simulated2DValues& c) const; +//}; +// +//class GaussianFactor { +// GaussianFactor(string key1, +// Matrix A1, +// Vector b_in, +// const SharedDiagonal& model); +// GaussianFactor(string key1, +// Matrix A1, +// string key2, +// Matrix A2, +// Vector b_in, +// const SharedDiagonal& model); +// GaussianFactor(string key1, +// Matrix A1, +// string key2, +// Matrix A2, +// string key3, +// Matrix A3, +// Vector b_in, +// const SharedDiagonal& model); +// bool involves(string key) const; +// Matrix getA(string key) const; +// pair matrix(const Ordering& ordering) const; +// pair eliminate(string key) const; +//}; +// +//class GaussianFactorGraph { +// GaussianConditional* eliminateOne(string key); +// GaussianBayesNet* eliminate_(const Ordering& ordering); +// VectorValues* optimize_(const Ordering& ordering); +// pair matrix(const Ordering& ordering) const; +// VectorValues* steepestDescent_(const VectorValues& x0) const; +// VectorValues* conjugateGradientDescent_(const VectorValues& x0) const; +//}; +// +//class Pose2Values{ +// Pose2Values(); +// Pose2 get(string key) const; +// void insert(string name, const Pose2& val); +// void print(string s) const; +// void clear(); +// int size(); +//}; +// +//class Pose2Factor { +// Pose2Factor(string key1, string key2, +// const Pose2& measured, Matrix measurement_covariance); +// void print(string name) const; +// double error(const Pose2Values& c) const; +// size_t size() const; +// GaussianFactor* linearize(const Pose2Values& config) const; +//}; +// +//class Pose2Graph{ +// Pose2Graph(); +// void print(string s) const; +// GaussianFactorGraph* linearize_(const Pose2Values& config) const; +// void push_back(Pose2Factor* factor); +//}; +// +//class SymbolicFactor{ +// SymbolicFactor(const Ordering& keys); +// void print(string s) const; +//}; +// +//class Simulated2DPosePrior { +// GaussianFactor* linearize(const Simulated2DValues& config) const; +//}; +// +//class Simulated2DOrientedPosePrior { +// GaussianFactor* linearize(const Simulated2DOrientedValues& config) const; +//}; +// +//class Simulated2DPointPrior { +// GaussianFactor* linearize(const Simulated2DValues& config) const; +//}; +// +//class Simulated2DOdometry { +// GaussianFactor* linearize(const Simulated2DValues& config) const; +//}; +// +//class Simulated2DOrientedOdometry { +// GaussianFactor* linearize(const Simulated2DOrientedValues& config) const; +//}; +// +//class Simulated2DMeasurement { +// GaussianFactor* linearize(const Simulated2DValues& config) const; +//}; +// +//class Pose2SLAMOptimizer { +// Pose2SLAMOptimizer(string dataset_name); +// void print(string s) const; +// void update(Vector x) const; +// Vector optimize() const; +// double error() const; +// Matrix a1() const; +// Matrix a2() const; +// Vector b1() const; +// Vector b2() const; +//}; diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 69fa02a45..46f738f7d 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -277,6 +277,18 @@ bool assert_container_equality(const V& expected, const V& actual) { return true; } +/** + * Compare strings for unit tests + */ +bool assert_equal(const std::string& expected, const std::string& actual) { + if (expected == actual) + return true; + printf("Not equal:\n"); + std::cout << "expected: [" << expected << "]\n"; + std::cout << "actual: [" << actual << "]" << std::endl; + return false; +} + /** * Allow for testing inequality */ diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 16f07f5ab..3f015e9d3 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -164,13 +164,14 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) if(debug) { cout << "Partial LDL with " << nFrontal << " frontal scalars, "; - print(ABC, "ABC: "); + print(ABC, "LDL ABC: "); } // Compute Cholesky factorization of A, overwrites A = sqrt(D)*R // tic(1, "ldl"); Eigen::LDLT ldlt; ldlt.compute(ABC.block(0,0,nFrontal,nFrontal).selfadjointView()); + if (debug) ldlt.isNegative() ? cout << "Matrix is negative" << endl : cout << "Matrix is not negative" << endl; if(ldlt.vectorD().unaryExpr(boost::bind(less(), _1, 0.0)).any()) { if(ISDEBUG("detailed_exceptions")) @@ -180,14 +181,15 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) } Vector sqrtD = ldlt.vectorD().cwiseSqrt(); // FIXME: we shouldn't do sqrt in LDL - if (debug) cout << "Dsqrt: " << sqrtD << endl; + if (debug) cout << "LDL Dsqrt:\n" << sqrtD << endl; // U = sqrtD * L^ Matrix U = ldlt.matrixU(); + if(debug) cout << "LDL U:\n" << U << endl; // we store the permuted upper triangular matrix ABC.block(0,0,nFrontal,nFrontal) = sqrtD.asDiagonal() * U; // FIXME: this isn't actually LDL', it's Cholesky - if(debug) cout << "R:\n" << ABC.topLeftCorner(nFrontal,nFrontal) << endl; + if(debug) cout << "LDL R:\n" << ABC.topLeftCorner(nFrontal,nFrontal) << endl; // toc(1, "ldl"); // Compute S = inv(R') * B = inv(P'U')*B = inv(U')*P*B @@ -196,20 +198,19 @@ Eigen::LDLT::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) ABC.topRightCorner(nFrontal, n-nFrontal) = ldlt.transpositionsP() * ABC.topRightCorner(nFrontal, n-nFrontal); ABC.block(0,0,nFrontal,nFrontal).triangularView().transpose().solveInPlace(ABC.topRightCorner(nFrontal, n-nFrontal)); } - if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl; + if(debug) cout << "LDL S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl; // toc(2, "compute S"); // Compute L = C - S' * S // tic(3, "compute L"); - if(debug) cout << "C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; + if(debug) cout << "LDL C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; if(n - nFrontal > 0) ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView().rankUpdate( ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0); - if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; + if(debug) cout << "LDL L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; // toc(3, "compute L"); - if(debug) cout << "P: " << ldlt.transpositionsP().indices() << endl; - + if(debug) cout << "LDL P: " << ldlt.transpositionsP().indices() << endl; return ldlt.transpositionsP(); } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index de311249a..74cb2453d 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -103,11 +103,6 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr compose_(const Pose2& p2) { - return boost::shared_ptr(new Pose2(compose(p2))); - } - /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { return Pose2(r_*p2.r(), t_ + r_*p2.t()); @@ -151,11 +146,6 @@ public: boost::optional H1=boost::none, boost::optional H2=boost::none) const; - /// MATLAB version returns shared pointer - boost::shared_ptr between_(const Pose2& p2) { - return boost::shared_ptr(new Pose2(between(p2))); - } - /** * Return point coordinates in pose coordinate frame */ diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index fa6f02828..6c6860f0f 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -38,7 +38,7 @@ namespace gtsam { // Calculate Adjoint map // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) // Experimental - unit tests of derivatives based on it do not check out yet - Matrix Pose3::AdjointMap() const { + Matrix Pose3::adjointMap() const { const Matrix R = R_.matrix(); const Vector t = t_.vector(); Matrix A = skewSymmetric(t)*R; @@ -188,7 +188,7 @@ namespace gtsam { boost::optional H1, boost::optional H2) const { if (H1) { #ifdef CORRECT_POSE3_EXMAP - *H1 = AdjointMap(inverse(p2)); // FIXME: this function doesn't exist with this interface + *H1 = adjointMap(inverse(p2)); // FIXME: this function doesn't exist with this interface #else const Rot3& R2 = p2.rotation(); const Point3& t2 = p2.translation(); @@ -214,22 +214,23 @@ namespace gtsam { /* ************************************************************************* */ Pose3 Pose3::inverse(boost::optional H1) const { - if (H1) + if (H1) #ifdef CORRECT_POSE3_EXMAP - { *H1 = - AdjointMap(p); } // FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ? + // FIXME: this function doesn't exist with this interface - should this be "*H1 = -adjointMap();" ? + { *H1 = - adjointMap(p); } #else - { - Matrix Rt = R_.transpose(); - Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3; - Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt; - Matrix DR = collect(2, &DR_R1, &DR_t1); - Matrix Dt = collect(2, &Dt_R1, &Dt_t1); - *H1 = gtsam::stack(2, &DR, &Dt); - } + { + Matrix Rt = R_.transpose(); + Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3; + Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt; + Matrix DR = collect(2, &DR_R1, &DR_t1); + Matrix Dt = collect(2, &Dt_R1, &Dt_t1); + *H1 = gtsam::stack(2, &DR, &Dt); + } #endif - Rot3 Rt = R_.inverse(); - return Pose3(Rt, Rt*(-t_)); - } + Rot3 Rt = R_.inverse(); + return Pose3(Rt, Rt*(-t_)); + } /* ************************************************************************* */ // between = compose(p2,inverse(p1)); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index f03dcde06..17b547e79 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -99,16 +99,16 @@ namespace gtsam { /// @{ /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes - static size_t Dim() { return dimension; } + static size_t Dim() { return dimension; } /// Dimensionality of the tangent space = 6 DOF size_t dim() const { return dimension; } - /** Exponential map around another pose */ /// Retraction from R^6 to Pose3 manifold neighborhood around current pose + /// Retraction from R^6 to Pose3 manifold neighborhood around current pose Pose3 retract(const Vector& d) const; - /// Logarithm map around another pose T1 /// Local 6D coordinates of Pose3 manifold neighborhood around current pose + /// Local 6D coordinates of Pose3 manifold neighborhood around current pose Vector localCoordinates(const Pose3& T2) const; /// @} @@ -148,8 +148,8 @@ namespace gtsam { * Calculate Adjoint map * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) */ - Matrix AdjointMap() const; - Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } + Matrix adjointMap() const; /// FIXME Not tested - marked as incorrect + Vector adjoint(const Vector& xi) const {return adjointMap()*xi; } /// FIXME Not tested - marked as incorrect /** * wedge for Pose3: diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 928b9b450..a4816914e 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -25,224 +25,234 @@ using namespace std; namespace gtsam { - /** Explicit instantiation of base class to export members */ - INSTANTIATE_LIE(Rot3M); +/** Explicit instantiation of base class to export members */ +INSTANTIATE_LIE(Rot3M); - static const Matrix I3 = eye(3); +static const Matrix I3 = eye(3); - /* ************************************************************************* */ - // static member functions to construct rotations +/* ************************************************************************* */ +Rot3M Rot3M::Rx(double t) { + double st = sin(t), ct = cos(t); + return Rot3M( + 1, 0, 0, + 0, ct,-st, + 0, st, ct); +} - Rot3M Rot3M::Rx(double t) { - double st = sin(t), ct = cos(t); - return Rot3M( - 1, 0, 0, - 0, ct,-st, - 0, st, ct); - } +/* ************************************************************************* */ +Rot3M Rot3M::Ry(double t) { + double st = sin(t), ct = cos(t); + return Rot3M( + ct, 0, st, + 0, 1, 0, + -st, 0, ct); +} - Rot3M Rot3M::Ry(double t) { - double st = sin(t), ct = cos(t); - return Rot3M( - ct, 0, st, - 0, 1, 0, - -st, 0, ct); - } +/* ************************************************************************* */ +Rot3M Rot3M::Rz(double t) { + double st = sin(t), ct = cos(t); + return Rot3M( + ct,-st, 0, + st, ct, 0, + 0, 0, 1); +} - Rot3M Rot3M::Rz(double t) { - double st = sin(t), ct = cos(t); - return Rot3M( - ct,-st, 0, - st, ct, 0, - 0, 0, 1); - } +/* ************************************************************************* */ +// Considerably faster than composing matrices above ! +Rot3M Rot3M::RzRyRx(double x, double y, double z) { + double cx=cos(x),sx=sin(x); + double cy=cos(y),sy=sin(y); + double cz=cos(z),sz=sin(z); + double ss_ = sx * sy; + double cs_ = cx * sy; + double sc_ = sx * cy; + double cc_ = cx * cy; + double c_s = cx * sz; + double s_s = sx * sz; + double _cs = cy * sz; + double _cc = cy * cz; + double s_c = sx * cz; + double c_c = cx * cz; + double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; + return Rot3M( + _cc,- c_s + ssc, s_s + csc, + _cs, c_c + sss, -s_c + css, + -sy, sc_, cc_ + ); +} - // Considerably faster than composing matrices above ! - Rot3M Rot3M::RzRyRx(double x, double y, double z) { - double cx=cos(x),sx=sin(x); - double cy=cos(y),sy=sin(y); - double cz=cos(z),sz=sin(z); - double ss_ = sx * sy; - double cs_ = cx * sy; - double sc_ = sx * cy; - double cc_ = cx * cy; - double c_s = cx * sz; - double s_s = sx * sz; - double _cs = cy * sz; - double _cc = cy * cz; - double s_c = sx * cz; - double c_c = cx * cz; - double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; - return Rot3M( - _cc,- c_s + ssc, s_s + csc, - _cs, c_c + sss, -s_c + css, - -sy, sc_, cc_ - ); - } - - /* ************************************************************************* */ - Rot3M Rot3M::rodriguez(const Vector& w, double theta) { - // get components of axis \omega - double wx = w(0), wy=w(1), wz=w(2); - double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; +/* ************************************************************************* */ +Rot3M Rot3M::rodriguez(const Vector& w, double theta) { + // get components of axis \omega + double wx = w(0), wy=w(1), wz=w(2); + double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; #ifndef NDEBUG - double l_n = wwTxx + wwTyy + wwTzz; - if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); + double l_n = wwTxx + wwTyy + wwTzz; + if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); #endif - double c = cos(theta), s = sin(theta), c_1 = 1 - c; + double c = cos(theta), s = sin(theta), c_1 = 1 - c; - double swx = wx * s, swy = wy * s, swz = wz * s; - double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; - double C11 = c_1*wwTyy, C12 = c_1*wy*wz; - double C22 = c_1*wwTzz; + double swx = wx * s, swy = wy * s, swz = wz * s; + double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; + double C11 = c_1*wwTyy, C12 = c_1*wy*wz; + double C22 = c_1*wwTzz; - return Rot3M( c + C00, -swz + C01, swy + C02, - swz + C01, c + C11, -swx + C12, - -swy + C02, swx + C12, c + C22); - } + return Rot3M( + c + C00, -swz + C01, swy + C02, + swz + C01, c + C11, -swx + C12, + -swy + C02, swx + C12, c + C22); +} - /* ************************************************************************* */ - Rot3M Rot3M::rodriguez(const Vector& w) { - double t = w.norm(); - if (t < 1e-10) return Rot3M(); - return rodriguez(w/t, t); - } +/* ************************************************************************* */ +Rot3M Rot3M::rodriguez(const Vector& w) { + double t = w.norm(); + if (t < 1e-10) return Rot3M(); + return rodriguez(w/t, t); +} - /* ************************************************************************* */ - bool Rot3M::equals(const Rot3M & R, double tol) const { - return equal_with_abs_tol(matrix(), R.matrix(), tol); - } +/* ************************************************************************* */ +bool Rot3M::equals(const Rot3M & R, double tol) const { + return equal_with_abs_tol(matrix(), R.matrix(), tol); +} - /* ************************************************************************* */ - Matrix Rot3M::matrix() const { - double r[] = { r1_.x(), r2_.x(), r3_.x(), - r1_.y(), r2_.y(), r3_.y(), - r1_.z(), r2_.z(), r3_.z() }; - return Matrix_(3,3, r); - } +/* ************************************************************************* */ +Matrix Rot3M::matrix() const { + Matrix R(3,3); + R << + r1_.x(), r2_.x(), r3_.x(), + r1_.y(), r2_.y(), r3_.y(), + r1_.z(), r2_.z(), r3_.z(); + return R; +} - /* ************************************************************************* */ - Matrix Rot3M::transpose() const { - double r[] = { r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()}; - return Matrix_(3,3, r); - } +/* ************************************************************************* */ +Matrix Rot3M::transpose() const { + Matrix Rt(3,3); + Rt << + r1_.x(), r1_.y(), r1_.z(), + r2_.x(), r2_.y(), r2_.z(), + r3_.x(), r3_.y(), r3_.z(); + return Rt; +} - /* ************************************************************************* */ - Point3 Rot3M::column(int index) const{ - if(index == 3) - return r3_; - else if (index == 2) - return r2_; - else - return r1_; // default returns r1 - } +/* ************************************************************************* */ +Point3 Rot3M::column(int index) const{ + if(index == 3) + return r3_; + else if (index == 2) + return r2_; + else + return r1_; // default returns r1 +} - /* ************************************************************************* */ - Vector Rot3M::xyz() const { - Matrix I;Vector q; - boost::tie(I,q)=RQ(matrix()); - return q; - } +/* ************************************************************************* */ +Vector Rot3M::xyz() const { + Matrix I;Vector q; + boost::tie(I,q)=RQ(matrix()); + return q; +} - Vector Rot3M::ypr() const { - Vector q = xyz(); - return Vector_(3,q(2),q(1),q(0)); - } +/* ************************************************************************* */ +Vector Rot3M::ypr() const { + Vector q = xyz(); + return Vector_(3,q(2),q(1),q(0)); +} - Vector Rot3M::rpy() const { - Vector q = xyz(); - return Vector_(3,q(0),q(1),q(2)); - } +/* ************************************************************************* */ +Vector Rot3M::rpy() const { + Vector q = xyz(); + return Vector_(3,q(0),q(1),q(2)); +} - /* ************************************************************************* */ - // Log map at identity - return the canonical coordinates of this rotation - Vector Rot3M::Logmap(const Rot3M& R) { - double tr = R.r1().x()+R.r2().y()+R.r3().z(); - if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) - return zero(3); - } else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3) - double theta = acos((tr-1.0)/2.0); - // Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4) - return (0.5 + theta*theta/12)*Vector_(3, - R.r2().z()-R.r3().y(), - R.r3().x()-R.r1().z(), - R.r1().y()-R.r2().x()); - } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. - if(fabs(R.r3().z() - -1.0) > 1e-10) - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r3().z())) * - Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); - else if(fabs(R.r2().y() - -1.0) > 1e-10) - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r2().y())) * - Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); - else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r1().x())) * - Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); - } else { - double theta = acos((tr-1.0)/2.0); - return (theta/2.0/sin(theta))*Vector_(3, - R.r2().z()-R.r3().y(), - R.r3().x()-R.r1().z(), - R.r1().y()-R.r2().x()); - } - } - - /* ************************************************************************* */ - Point3 Rot3M::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = matrix(); - return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); - } - - /* ************************************************************************* */ - // see doc/math.lyx, SO(3) section - Point3 Rot3M::unrotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - const Matrix Rt(transpose()); - Point3 q(Rt*p.vector()); // q = Rt*p - if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); - if (H2) *H2 = Rt; - return q; - } - - /* ************************************************************************* */ - Rot3M Rot3M::compose (const Rot3M& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; - } - - /* ************************************************************************* */ - Rot3M Rot3M::between (const Rot3M& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*matrix()); - if (H2) *H2 = I3; - return between_default(*this, R2); - } - - /* ************************************************************************* */ - pair RQ(const Matrix& A) { - - double x = -atan2(-A(2, 1), A(2, 2)); - Rot3M Qx = Rot3M::Rx(-x); - Matrix B = A * Qx.matrix(); - - double y = -atan2(B(2, 0), B(2, 2)); - Rot3M Qy = Rot3M::Ry(-y); - Matrix C = B * Qy.matrix(); - - double z = -atan2(-C(1, 0), C(1, 1)); - Rot3M Qz = Rot3M::Rz(-z); - Matrix R = C * Qz.matrix(); - - Vector xyz = Vector_(3, x, y, z); - return make_pair(R, xyz); +/* ************************************************************************* */ +// Log map at identity - return the canonical coordinates of this rotation +Vector Rot3M::Logmap(const Rot3M& R) { + double tr = R.r1().x()+R.r2().y()+R.r3().z(); + // FIXME should tr in statement below be absolute value? + if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) + return zero(3); + } else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3) + double theta = acos((tr-1.0)/2.0); + // Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4) + return (0.5 + theta*theta/12)*Vector_(3, + R.r2().z()-R.r3().y(), + R.r3().x()-R.r1().z(), + R.r1().y()-R.r2().x()); + // FIXME: in statement below, is this the right comparision? + } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. + if(fabs(R.r3().z() - -1.0) > 1e-10) + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r3().z())) * + Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); + else if(fabs(R.r2().y() - -1.0) > 1e-10) + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r2().y())) * + Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); + else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r1().x())) * + Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); + } else { + double theta = acos((tr-1.0)/2.0); + return (theta/2.0/sin(theta))*Vector_(3, + R.r2().z()-R.r3().y(), + R.r3().x()-R.r1().z(), + R.r1().y()-R.r2().x()); } +} - /* ************************************************************************* */ +/* ************************************************************************* */ +Point3 Rot3M::rotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = matrix(); + return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); +} + +/* ************************************************************************* */ +// see doc/math.lyx, SO(3) section +Point3 Rot3M::unrotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + const Matrix Rt(transpose()); + Point3 q(Rt*p.vector()); // q = Rt*p + if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); + if (H2) *H2 = Rt; + return q; +} + +/* ************************************************************************* */ +Rot3M Rot3M::compose (const Rot3M& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return *this * R2; +} + +/* ************************************************************************* */ +Rot3M Rot3M::between (const Rot3M& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = -(R2.transpose()*matrix()); + if (H2) *H2 = I3; + return between_default(*this, R2); +} + +/* ************************************************************************* */ +pair RQ(const Matrix& A) { + + double x = -atan2(-A(2, 1), A(2, 2)); + Rot3M Qx = Rot3M::Rx(-x); + Matrix B = A * Qx.matrix(); + + double y = -atan2(B(2, 0), B(2, 2)); + Rot3M Qy = Rot3M::Ry(-y); + Matrix C = B * Qy.matrix(); + + double z = -atan2(-C(1, 0), C(1, 1)); + Rot3M Qz = Rot3M::Rz(-z); + Matrix R = C * Qz.matrix(); + + Vector xyz = Vector_(3, x, y, z); + return make_pair(R, xyz); +} + +/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Rot3M.h b/gtsam/geometry/Rot3M.h index e41e01ef3..02ca733d1 100644 --- a/gtsam/geometry/Rot3M.h +++ b/gtsam/geometry/Rot3M.h @@ -226,10 +226,20 @@ namespace gtsam { /** * Use RQ to calculate roll-pitch-yaw angle representation - * @return a vector containing ypr s.t. R = Rot3M::ypr(y,p,r) + * @return a vector containing rpy s.t. R = Rot3M::ypr(y,p,r) */ Vector rpy() const; + /** + * Accessors to get to components of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient + */ + inline double roll() const { return ypr()(2); } + inline double pitch() const { return ypr()(1); } + inline double yaw() const { return ypr()(0); } + /** Compute the quaternion representation of this rotation. * @return The quaternion */ diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index cb336bb7e..94b81cb0c 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -51,7 +51,6 @@ TEST( Pose3, expmap_a) v(0) = 0.3; EXPECT(assert_equal(id.retract(v), Pose3(R, Point3()))); #ifdef CORRECT_POSE3_EXMAP - v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; #else v(3)=0.2;v(4)=0.7;v(5)=-2; @@ -100,89 +99,6 @@ namespace screw { Pose3 expected(expectedR, expectedT); } -#ifdef CORRECT_POSE3_EXMAP -/* ************************************************************************* */ -TEST(Pose3, expmap_c) -{ - EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); - EXPECT(assert_equal(screw::expected, Pose3::Expmap(screw::xi),1e-6)); -} - -/* ************************************************************************* */ -// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) -TEST(Pose3, Adjoint) -{ - Pose3 expected = T * Pose3::Expmap(screw::xi) * inverse(T); - Vector xiprime = Adjoint(T, screw::xi); - EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); - - Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * inverse(T2); - Vector xiprime2 = Adjoint(T2, screw::xi); - EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); - - Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * inverse(T3); - Vector xiprime3 = Adjoint(T3, screw::xi); - EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); -} - -/* ************************************************************************* */ -/** Agrawal06iros version */ -Pose3 Agrawal06iros(const Vector& xi) { - Vector w = vector_range(xi, range(0,3)); - Vector v = vector_range(xi, range(3,6)); - double t = norm_2(w); - if (t < 1e-5) - return Pose3(Rot3(), expmap (v)); - else { - Matrix W = skewSymmetric(w/t); - Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); - return Pose3(Rot3::Expmap (w), expmap (A * v)); - } -} - -/* ************************************************************************* */ -TEST(Pose3, expmaps_galore) -{ - Vector xi; Pose3 actual; - xi = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); - actual = Pose3::Expmap(xi); - EXPECT(assert_equal(expm(xi), actual,1e-6)); - EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); - EXPECT(assert_equal(xi, localCoordinates(actual),1e-6)); - - xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6); - for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { - Vector txi = xi*theta; - actual = Pose3::Expmap(txi); - EXPECT(assert_equal(expm(txi,30), actual,1e-6)); - EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6)); - Vector log = localCoordinates(actual); - EXPECT(assert_equal(actual, Pose3::Expmap(log),1e-6)); - EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps - } - - // Works with large v as well, but expm needs 10 iterations! - xi = Vector_(6,0.2,0.3,-0.8,100.0,120.0,-60.0); - actual = Pose3::Expmap(xi); - EXPECT(assert_equal(expm(xi,10), actual,1e-5)); - EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); - EXPECT(assert_equal(xi, localCoordinates(actual),1e-6)); -} - -/* ************************************************************************* */ -TEST(Pose3, Adjoint_compose) -{ - // To debug derivatives of compose, assert that - // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 - const Pose3& T1 = T; - Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); - Pose3 expected = T1 * Pose3::Expmap(x) * T2; - Vector y = Adjoint(inverse(T2), x); - Pose3 actual = T1 * T2 * Pose3::Expmap(y); - EXPECT(assert_equal(expected, actual, 1e-6)); -} -#endif // SLOW_BUT_CORRECT_EXMAP - /* ************************************************************************* */ TEST(Pose3, expmap_c_full) { @@ -195,15 +111,15 @@ TEST(Pose3, expmap_c_full) TEST(Pose3, Adjoint_full) { Pose3 expected = T * Pose3::Expmap(screw::xi) * T.inverse(); - Vector xiprime = T.Adjoint(screw::xi); + Vector xiprime = T.adjoint(screw::xi); EXPECT(assert_equal(expected, Pose3::Expmap(xiprime), 1e-6)); Pose3 expected2 = T2 * Pose3::Expmap(screw::xi) * T2.inverse(); - Vector xiprime2 = T2.Adjoint(screw::xi); + Vector xiprime2 = T2.adjoint(screw::xi); EXPECT(assert_equal(expected2, Pose3::Expmap(xiprime2), 1e-6)); Pose3 expected3 = T3 * Pose3::Expmap(screw::xi) * T3.inverse(); - Vector xiprime3 = T3.Adjoint(screw::xi); + Vector xiprime3 = T3.adjoint(screw::xi); EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); } @@ -259,7 +175,7 @@ TEST(Pose3, Adjoint_compose_full) const Pose3& T1 = T; Vector x = Vector_(6,0.1,0.1,0.1,0.4,0.2,0.8); Pose3 expected = T1 * Pose3::Expmap(x) * T2; - Vector y = T2.inverse().Adjoint(x); + Vector y = T2.inverse().adjoint(x); Pose3 actual = T1 * T2 * Pose3::Expmap(y); EXPECT(assert_equal(expected, actual, 1e-6)); } diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 2f6b9735e..8cb9e3278 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -178,6 +178,11 @@ namespace gtsam { /** check equality */ bool equals(const BayesTree& other, double tol = 1e-9) const; + /** deep copy from another tree */ + void cloneTo(shared_ptr& newTree) const { + root_->cloneToBayesTree(*newTree); + } + /** * Find parent clique of a conditional. It will look at all parents and * return the one with the lowest index in the ordering. diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 549814312..83496dfc0 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -77,6 +77,20 @@ namespace gtsam { */ static derived_ptr Create(const std::pair >& result) { return boost::make_shared(result); } + void cloneToBayesTree(BayesTree& newTree, shared_ptr parent_clique = shared_ptr()) const { + sharedConditional newConditional = sharedConditional(new CONDITIONAL(*conditional_)); + sharedClique newClique = newTree.addClique(newConditional, parent_clique); + if (cachedFactor_) + newClique->cachedFactor_ = cachedFactor_->clone(); + else newClique->cachedFactor_ = typename FactorType::shared_ptr(); + if (!parent_clique) { + newTree.root_ = newClique; + } + BOOST_FOREACH(const shared_ptr& childClique, children_) { + childClique->cloneToBayesTree(newTree, newClique); + } + } + /** print this node */ void print(const std::string& s = "") const; diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 421d03ad5..99d717681 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -174,6 +174,13 @@ public: /** 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); } diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 7f4b90748..2d889ee64 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -88,6 +88,9 @@ namespace gtsam { /** Return the dimension of the variable pointed to by the given key iterator */ virtual size_t getDim(const_iterator variable) const = 0; + /** Clone a factor (make a deep copy) */ + virtual GaussianFactor::shared_ptr clone() const = 0; + /** * Permutes the GaussianFactor, but for efficiency requires the permutation * to already be inverted. This acts just as a change-of-name for each diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index f2de0be4c..88e4364ae 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -191,6 +191,12 @@ namespace gtsam { /** Destructor */ virtual ~HessianFactor() {} + /** Clone this JacobianFactor */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + shared_ptr(new HessianFactor(*this))); + } + /** Print the factor for debugging and testing (implementing Testable) */ virtual void print(const std::string& s = "") const; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index b8f40d6ef..09089df20 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -136,6 +136,12 @@ namespace gtsam { virtual ~JacobianFactor() {} + /** Clone this JacobianFactor */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + shared_ptr(new JacobianFactor(*this))); + } + // Implementing Testable interface virtual void print(const std::string& s = "") const; virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index d4d177815..fe29936cb 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -27,12 +27,18 @@ namespace gtsam { */ template > class GaussianISAM2 : public ISAM2 { + typedef ISAM2 Base; public: /** Create an empty ISAM2 instance */ GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ GaussianISAM2() : ISAM2() {} + + void cloneTo(boost::shared_ptr& newGaussianISAM2) const { + boost::shared_ptr isam2 = boost::static_pointer_cast(newGaussianISAM2); + Base::cloneTo(isam2); + } }; // optimize the BayesTree, starting from the root diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 3e3d34996..f3e960db4 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -214,6 +214,27 @@ public: typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class + void cloneTo(boost::shared_ptr& newISAM2) const { + boost::shared_ptr bayesTree = boost::static_pointer_cast(newISAM2); + Base::cloneTo(bayesTree); + newISAM2->theta_ = theta_; + newISAM2->variableIndex_ = variableIndex_; + newISAM2->deltaUnpermuted_ = deltaUnpermuted_; + newISAM2->delta_ = delta_; + newISAM2->nonlinearFactors_ = nonlinearFactors_; + newISAM2->ordering_ = ordering_; + newISAM2->params_ = params_; +#ifndef NDEBUG + newISAM2->lastRelinVariables_ = lastRelinVariables_; +#endif + newISAM2->lastAffectedVariableCount = lastAffectedVariableCount; + newISAM2->lastAffectedFactorCount = lastAffectedFactorCount; + newISAM2->lastAffectedCliqueCount = lastAffectedCliqueCount; + newISAM2->lastAffectedMarkedCount = lastAffectedMarkedCount; + newISAM2->lastBacksubVariableCount = lastBacksubVariableCount; + newISAM2->lastNnzTop = lastNnzTop; + } + /** * Add new factors, updating the solution and relinearizing as needed. * @@ -279,6 +300,8 @@ public: size_t lastBacksubVariableCount; size_t lastNnzTop; + ISAM2Params params() const { return params_; } + //@} private: diff --git a/gtsam/slam/Landmark2.h b/gtsam/slam/Landmark2.h deleted file mode 100644 index f67cfed56..000000000 --- a/gtsam/slam/Landmark2.h +++ /dev/null @@ -1,27 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Landmark2.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - typedef gtsam::Point2 Landmark2; - -} - diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index 406431edd..fa51a9512 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -15,24 +15,12 @@ headers += simulated2DConstraints.h sources += simulated2D.cpp smallExample.cpp check_PROGRAMS += tests/testSimulated2D -# MATLAB Wrap headers for simulated2D -headers += Simulated2DMeasurement.h -headers += Simulated2DOdometry.h -headers += Simulated2DPointPrior.h -headers += Simulated2DPosePrior.h -headers += Simulated2DValues.h - # simulated2DOriented example sources += simulated2DOriented.cpp check_PROGRAMS += tests/testSimulated2DOriented -# MATLAB Wrap headers for simulated2DOriented -headers += Simulated2DOrientedOdometry.h -headers += Simulated2DOrientedPosePrior.h -headers += Simulated2DOrientedValues.h - # simulated3D example -sources += Simulated3D.cpp +sources += simulated3D.cpp check_PROGRAMS += tests/testSimulated3D # Generic SLAM headers @@ -50,12 +38,6 @@ check_PROGRAMS += tests/testPose2SLAM sources += planarSLAM.cpp check_PROGRAMS += tests/testPlanarSLAM -# MATLAB Wrap headers for planarSLAM -headers += Landmark2.h -headers += PlanarSLAMGraph.h -headers += PlanarSLAMValues.h -headers += PlanarSLAMOdometry.h - # 3D Pose constraints sources += pose3SLAM.cpp check_PROGRAMS += tests/testPose3SLAM diff --git a/gtsam/slam/PlanarSLAMGraph.h b/gtsam/slam/PlanarSLAMGraph.h deleted file mode 100644 index 98eb9aaef..000000000 --- a/gtsam/slam/PlanarSLAMGraph.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 PlanarSLAMGraph.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef gtsam::planarSLAM::Graph PlanarSLAMGraph; - -} - diff --git a/gtsam/slam/PlanarSLAMOdometry.h b/gtsam/slam/PlanarSLAMOdometry.h deleted file mode 100644 index 941eda285..000000000 --- a/gtsam/slam/PlanarSLAMOdometry.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 PlanarSLAMOdometry.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef gtsam::planarSLAM::Odometry PlanarSLAMOdometry; - -} - diff --git a/gtsam/slam/PlanarSLAMValues.h b/gtsam/slam/PlanarSLAMValues.h deleted file mode 100644 index d174229fd..000000000 --- a/gtsam/slam/PlanarSLAMValues.h +++ /dev/null @@ -1,27 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 PlanarSLAMValues.h - * @brief Convenience for MATLAB wrapper, which does not allow for identically named methods - * @author Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - typedef gtsam::planarSLAM::Values PlanarSLAMValues; - -} - diff --git a/gtsam/slam/Simulated2DMeasurement.h b/gtsam/slam/Simulated2DMeasurement.h deleted file mode 100644 index 0ab661086..000000000 --- a/gtsam/slam/Simulated2DMeasurement.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DMeasurement.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef simulated2D::Measurement Simulated2DMeasurement; - -} - diff --git a/gtsam/slam/Simulated2DOdometry.h b/gtsam/slam/Simulated2DOdometry.h deleted file mode 100644 index 5bd3afc14..000000000 --- a/gtsam/slam/Simulated2DOdometry.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DOdometry.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef simulated2D::Odometry Simulated2DOdometry; - -} - diff --git a/gtsam/slam/Simulated2DOrientedOdometry.h b/gtsam/slam/Simulated2DOrientedOdometry.h deleted file mode 100644 index 1a659c081..000000000 --- a/gtsam/slam/Simulated2DOrientedOdometry.h +++ /dev/null @@ -1,28 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DOrientedOdometry.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Kai Ni - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef simulated2DOriented::Odometry Simulated2DOrientedOdometry; - -} - diff --git a/gtsam/slam/Simulated2DOrientedPosePrior.h b/gtsam/slam/Simulated2DOrientedPosePrior.h deleted file mode 100644 index 54a83f1be..000000000 --- a/gtsam/slam/Simulated2DOrientedPosePrior.h +++ /dev/null @@ -1,29 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DOrientedPosePrior.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Kai Ni - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** Create a prior on a pose Point2 with key 'x1' etc... */ - typedef simulated2DOriented::GenericPosePrior Simulated2DOrientedPosePrior; - -} - diff --git a/gtsam/slam/Simulated2DOrientedValues.h b/gtsam/slam/Simulated2DOrientedValues.h deleted file mode 100644 index b1fe098f8..000000000 --- a/gtsam/slam/Simulated2DOrientedValues.h +++ /dev/null @@ -1,59 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DValues.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - class Simulated2DOrientedValues: public simulated2DOriented::Values { - public: - typedef boost::shared_ptr sharedPoint; - typedef boost::shared_ptr sharedPose; - - Simulated2DOrientedValues() { - } - - void insertPose(const simulated2DOriented::PoseKey& i, const Pose2& p) { - insert(i, p); - } - - void insertPoint(const simulated2DOriented::PointKey& j, const Point2& p) { - insert(j, p); - } - - int nrPoses() const { - return this->first_.size(); - } - - int nrPoints() const { - return this->second_.size(); - } - - sharedPose pose(const simulated2DOriented::PoseKey& i) { - return sharedPose(new Pose2((*this)[i])); - } - - sharedPoint point(const simulated2DOriented::PointKey& j) { - return sharedPoint(new Point2((*this)[j])); - } - - }; - -} // namespace gtsam - diff --git a/gtsam/slam/Simulated2DPointPrior.h b/gtsam/slam/Simulated2DPointPrior.h deleted file mode 100644 index 8fc896d92..000000000 --- a/gtsam/slam/Simulated2DPointPrior.h +++ /dev/null @@ -1,29 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DPointPrior.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** Create a prior on a landmark Point2 with key 'l1' etc... */ - typedef simulated2D::GenericPrior Simulated2DPointPrior; - -} - diff --git a/gtsam/slam/Simulated2DPosePrior.h b/gtsam/slam/Simulated2DPosePrior.h deleted file mode 100644 index 1c85e4d5b..000000000 --- a/gtsam/slam/Simulated2DPosePrior.h +++ /dev/null @@ -1,29 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DPosePrior.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** Create a prior on a pose Point2 with key 'x1' etc... */ - typedef simulated2D::GenericPrior Simulated2DPosePrior; - -} - diff --git a/gtsam/slam/Simulated2DValues.h b/gtsam/slam/Simulated2DValues.h deleted file mode 100644 index c4d1d1534..000000000 --- a/gtsam/slam/Simulated2DValues.h +++ /dev/null @@ -1,58 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated2DValues.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - class Simulated2DValues: public simulated2D::Values { - public: - typedef boost::shared_ptr sharedPoint; - - Simulated2DValues() { - } - - void insertPose(const simulated2D::PoseKey& i, const Point2& p) { - insert(i, p); - } - - void insertPoint(const simulated2D::PointKey& j, const Point2& p) { - insert(j, p); - } - - int nrPoses() const { - return this->first_.size(); - } - - int nrPoints() const { - return this->second_.size(); - } - - sharedPoint pose(const simulated2D::PoseKey& i) { - return sharedPoint(new Point2((*this)[i])); - } - - sharedPoint point(const simulated2D::PointKey& j) { - return sharedPoint(new Point2((*this)[j])); - } - - }; - -} // namespace gtsam - diff --git a/gtsam/slam/Simulated3D.cpp b/gtsam/slam/Simulated3D.cpp index 76e65a8cf..e2f6211a5 100644 --- a/gtsam/slam/Simulated3D.cpp +++ b/gtsam/slam/Simulated3D.cpp @@ -15,7 +15,7 @@ * @author Alex Cunningham **/ -#include +#include namespace gtsam { diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 228ced08c..5776e989c 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -71,10 +71,10 @@ namespace gtsam { // Convenience for MATLAB wrapper, which does not allow for identically named methods /// get a pose - boost::shared_ptr pose(int key) { - Pose2 pose = (*this)[PoseKey(key)]; - return boost::shared_ptr(new Pose2(pose)); - } + Pose2 pose(int key) const { return (*this)[PoseKey(key)]; } + + /// get a point + Point2 point(int key) const { return (*this)[PointKey(key)]; } /// insert a pose void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); } @@ -131,14 +131,11 @@ namespace gtsam { void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing, double range, const SharedNoiseModel& model); - /// Optimize_ for MATLAB - boost::shared_ptr optimize_(const Values& initialEstimate) { + /// Optimize + Values optimize(const Values& initialEstimate) { typedef NonlinearOptimizer Optimizer; - boost::shared_ptr result( - new Values( - *Optimizer::optimizeLM(*this, initialEstimate, - NonlinearOptimizationParameters::LAMBDA))); - return result; + return *Optimizer::optimizeLM(*this, initialEstimate, + NonlinearOptimizationParameters::LAMBDA); } }; diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index 3b63c9595..cae60bb80 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -21,6 +21,7 @@ #include #include #include +#include // \namespace @@ -72,13 +73,13 @@ namespace gtsam { } /// Return pose i - sharedPoint pose(const simulated2D::PoseKey& i) { - return sharedPoint(new Point2((*this)[i])); + Point2 pose(const simulated2D::PoseKey& i) const { + return (*this)[i]; } /// Return point j - sharedPoint point(const simulated2D::PointKey& j) { - return sharedPoint(new Point2((*this)[j])); + Point2 point(const simulated2D::PointKey& j) const { + return (*this)[j]; } }; @@ -232,5 +233,12 @@ namespace gtsam { typedef GenericOdometry Odometry; typedef GenericMeasurement Measurement; + // Specialization of a graph for this example domain + // TODO: add functions to add factor types + class Graph : public NonlinearFactorGraph { + public: + Graph() {} + }; + } // namespace simulated2D } // namespace gtsam diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index 3710f29ad..b9cf150ef 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -21,6 +21,7 @@ #include #include #include +#include // \namespace @@ -33,7 +34,27 @@ namespace gtsam { typedef TypedSymbol PoseKey; typedef Values PoseValues; typedef Values PointValues; - typedef TupleValues2 Values; + + /// Specialized Values structure with syntactic sugar for + /// compatibility with matlab + class Values: public TupleValues2 { + public: + Values() {} + + void insertPose(const PoseKey& i, const Pose2& p) { + insert(i, p); + } + + void insertPoint(const PointKey& j, const Point2& p) { + insert(j, p); + } + + int nrPoses() const { return this->first_.size(); } + int nrPoints() const { return this->second_.size(); } + + Pose2 pose(const PoseKey& i) const { return (*this)[i]; } + Point2 point(const PointKey& j) const { return (*this)[j]; } + }; //TODO:: point prior is not implemented right now @@ -100,5 +121,12 @@ namespace gtsam { typedef GenericOdometry Odometry; + /// Graph specialization for syntactic sugar use with matlab + class Graph : public NonlinearFactorGraph { + public: + Graph() {} + // TODO: add functions to add factors + }; + } // namespace simulated2DOriented } // namespace gtsam diff --git a/gtsam/slam/simulated3D.cpp b/gtsam/slam/simulated3D.cpp new file mode 100644 index 000000000..e2f6211a5 --- /dev/null +++ b/gtsam/slam/simulated3D.cpp @@ -0,0 +1,43 @@ +/* ---------------------------------------------------------------------------- + + * 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 Simulated3D.cpp +* @brief measurement functions and derivatives for simulated 3D robot +* @author Alex Cunningham +**/ + +#include + +namespace gtsam { + +namespace simulated3D { + +Point3 prior (const Point3& x, boost::optional H) { + if (H) *H = eye(3); + return x; +} + +Point3 odo(const Point3& x1, const Point3& x2, + boost::optional H1, boost::optional H2) { + if (H1) *H1 = -1 * eye(3); + if (H2) *H2 = eye(3); + return x2 - x1; +} + +Point3 mea(const Point3& x, const Point3& l, + boost::optional H1, boost::optional H2) { + if (H1) *H1 = -1 * eye(3); + if (H2) *H2 = eye(3); + return l - x; +} + +}} // namespace gtsam::simulated3D diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h new file mode 100644 index 000000000..32f05e967 --- /dev/null +++ b/gtsam/slam/simulated3D.h @@ -0,0 +1,132 @@ +/* ---------------------------------------------------------------------------- + + * 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 Simulated3D.h + * @brief measurement functions and derivatives for simulated 3D robot + * @author Alex Cunningham + **/ + +// \callgraph + +#pragma once + +#include +#include +#include +#include +#include +#include + +// \namespace + +namespace gtsam { +namespace simulated3D { + +/** + * This is a linear SLAM domain where both poses and landmarks are + * 3D points, without rotation. The structure and use is based on + * the simulated2D domain. + */ + +typedef gtsam::TypedSymbol PoseKey; +typedef gtsam::TypedSymbol PointKey; + +typedef Values PoseValues; +typedef Values PointValues; +typedef TupleValues2 Values; + +/** + * Prior on a single pose + */ +Point3 prior(const Point3& x, boost::optional H = boost::none); + +/** + * odometry between two poses + */ +Point3 odo(const Point3& x1, const Point3& x2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none); + +/** + * measurement between landmark and pose + */ +Point3 mea(const Point3& x, const Point3& l, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none); + +/** + * A prior factor on a single linear robot pose + */ +struct PointPrior3D: public NonlinearFactor1 { + + Point3 z_; ///< The prior pose value for the variable attached to this factor + + /** + * Constructor for a prior factor + * @param z is the measured/prior position for the pose + * @param model is the measurement model for the factor (Dimension: 3) + * @param j is the key for the pose + */ + PointPrior3D(const Point3& z, + const SharedNoiseModel& model, const PoseKey& j) : + NonlinearFactor1 (model, j), z_(z) { + } + + /** + * Evaluates the error at a given value of x, + * with optional derivatives. + * @param x is the current value of the variable + * @param H is an optional Jacobian matrix (Dimension: 3x3) + * @return Vector error between prior value and x (Dimension: 3) + */ + Vector evaluateError(const Point3& x, boost::optional H = + boost::none) { + return (prior(x, H) - z_).vector(); + } +}; + +/** + * Models a linear 3D measurement between 3D points + */ +struct Simulated3DMeasurement: public NonlinearFactor2 { + + Point3 z_; ///< Linear displacement between a pose and landmark + + /** + * Creates a measurement factor with a given measurement + * @param z is the measurement, a linear displacement between poses and landmarks + * @param model is a measurement model for the factor (Dimension: 3) + * @param j1 is the pose key of the robot + * @param j2 is the point key for the landmark + */ + Simulated3DMeasurement(const Point3& z, + const SharedNoiseModel& model, PoseKey& j1, PointKey j2) : + NonlinearFactor2 ( + model, j1, j2), z_(z) { + } + + /** + * Error function with optional derivatives + * @param x1 a robot pose value + * @param x2 a landmark point value + * @param H1 is an optional Jacobian matrix in terms of x1 (Dimension: 3x3) + * @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3) + * @return vector error between measurement and prediction (Dimension: 3) + */ + Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional< + Matrix&> H1 = boost::none, boost::optional H2 = boost::none) { + return (mea(x1, x2, H1, H2) - z_).vector(); + } +}; + +}} // namespace simulated3D diff --git a/gtsam/slam/tests/testSimulated3D.cpp b/gtsam/slam/tests/testSimulated3D.cpp index de2c534bb..30f41e2fd 100644 --- a/gtsam/slam/tests/testSimulated3D.cpp +++ b/gtsam/slam/tests/testSimulated3D.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include using namespace gtsam; diff --git a/myconfigure.matlab b/myconfigure.matlab index 33addb665..a94644b77 100755 --- a/myconfigure.matlab +++ b/myconfigure.matlab @@ -1 +1,12 @@ -../configure --prefix=$HOME --with-toolbox=$HOME/toolbox --enable-build-toolbox CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static --disable-fast-install +../configure --prefix=$HOME \ + --with-toolbox=$HOME/toolbox \ + --enable-build-toolbox \ + --enable-install-matlab-tests \ + --enable-install-matlab-examples \ + --enable-install-wrap \ + --with-wrap=$HOME/bin \ + CFLAGS="-fno-inline -g -Wall" \ + CXXFLAGS="-fno-inline -g -Wall" \ + LDFLAGS="-fno-inline -g -Wall" \ + --disable-static \ + --disable-fast-install diff --git a/tests/matlab/test_gtsam.m b/tests/matlab/test_gtsam.m new file mode 100644 index 000000000..aab3dac0f --- /dev/null +++ b/tests/matlab/test_gtsam.m @@ -0,0 +1,10 @@ +% Test runner script - runs each test + +display 'Starting: testJacobianFactor' +testJacobianFactor + +display 'Starting: testKalmanFilter' +testKalmanFilter + +% end of tests +display 'Tests complete!' diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 378afade3..121717cf3 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -292,6 +292,110 @@ TEST(ISAM2, slamlike_solution) EXPECT(isam_check(fullgraph, fullinit, isam)); } +/* ************************************************************************* */ +TEST_UNSAFE(ISAM2, clone) { + + // Pose and landmark key types from planarSLAM + typedef planarSLAM::PoseKey PoseKey; + typedef planarSLAM::PointKey PointKey; + + // Set up parameters + SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); + SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); + + // These variables will be reused and accumulate factors and values + GaussianISAM2 isam(ISAM2Params(0.001, 0.0, 0, false, true)); + planarSLAM::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.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + + isam.update(newfactors, init); + } + + EXPECT(isam_check(fullgraph, fullinit, isam)); + + // Add odometry from time 0 to time 5 + for( ; i<5; ++i) { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(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.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + fullinit.insert(PointKey(1), 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.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(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.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + + isam.update(newfactors, init); + ++ i; + } + + // CLONING... + boost::shared_ptr > isam2 + = boost::shared_ptr >(new GaussianISAM2()); + isam.cloneTo(isam2); + + CHECK(assert_equal(isam, *isam2)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index e0d01ab63..1610f98aa 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -22,34 +22,43 @@ #include "Argument.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ -void Argument::matlab_unwrap(ofstream& ofs, - const string& matlabName) -{ +void Argument::matlab_unwrap(ofstream& ofs, const string& matlabName) const { ofs << " "; + string cppType = qualifiedType("::"); + string matlabType = qualifiedType(); + if (is_ptr) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer - ofs << "shared_ptr<" << type << "> " << name << " = unwrap_shared_ptr< "; + ofs << "shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; else if (is_ref) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer - ofs << type << "& " << name << " = *unwrap_shared_ptr< "; + ofs << 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]); - ofs << type << " " << name << " = unwrap< "; + ofs << cppType << " " << name << " = unwrap< "; - ofs << type << " >(" << matlabName; - if (is_ptr || is_ref) ofs << ", \"" << type << "\""; + ofs << cppType << " >(" << matlabName; + if (is_ptr || is_ref) ofs << ", \"" << matlabType << "\""; ofs << ");" << endl; } /* ************************************************************************* */ -string ArgumentList::types() { +string Argument::qualifiedType(const string& delim) const { + string result; + BOOST_FOREACH(const string& ns, namespaces) result += ns + delim; + return result + type; +} + +/* ************************************************************************* */ +string ArgumentList::types() const { string str; bool first=true; BOOST_FOREACH(Argument arg, *this) { @@ -59,7 +68,7 @@ string ArgumentList::types() { } /* ************************************************************************* */ -string ArgumentList::signature() { +string ArgumentList::signature() const { string str; BOOST_FOREACH(Argument arg, *this) str += arg.type[0]; @@ -67,7 +76,7 @@ string ArgumentList::signature() { } /* ************************************************************************* */ -string ArgumentList::names() { +string ArgumentList::names() const { string str; bool first=true; BOOST_FOREACH(Argument arg, *this) { @@ -77,7 +86,7 @@ string ArgumentList::names() { } /* ************************************************************************* */ -void ArgumentList::matlab_unwrap(ofstream& ofs, int start) { +void ArgumentList::matlab_unwrap(ofstream& ofs, int start) const { int index = start; BOOST_FOREACH(Argument arg, *this) { stringstream buf; diff --git a/wrap/Argument.h b/wrap/Argument.h index 2248e1790..cb5b1b8c8 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -18,27 +18,32 @@ #pragma once #include -#include +#include + +namespace wrap { /// Argument class struct Argument { bool is_const, is_ref, is_ptr; std::string type; std::string name; + std::vector namespaces; Argument() : is_const(false), is_ref(false), is_ptr(false) { } + std::string qualifiedType(const std::string& delim = "") const; // adds namespaces to type + /// MATLAB code generation, MATLAB to C++ - void matlab_unwrap(std::ofstream& ofs, const std::string& matlabName); + void matlab_unwrap(std::ofstream& ofs, const std::string& matlabName) const; }; /// Argument list -struct ArgumentList: public std::list { - std::list args; - std::string types(); - std::string signature(); - std::string names(); +struct ArgumentList: public std::vector { + std::vector args; // why does it contain this? + std::string types() const; + std::string signature() const; + std::string names() const; // MATLAB code generation: @@ -47,6 +52,8 @@ struct ArgumentList: public std::list { * @param ofs output stream * @param start initial index for input array, set to 1 for method */ - void matlab_unwrap(std::ofstream& ofs, int start = 0); // MATLAB to C++ + void matlab_unwrap(std::ofstream& ofs, int start = 0) const; // MATLAB to C++ }; +} // \namespace wrap + diff --git a/wrap/Class.cpp b/wrap/Class.cpp index d701731d8..47e01f00b 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -14,6 +14,7 @@ * @author Frank Dellaert **/ +#include #include #include @@ -23,24 +24,28 @@ #include "utilities.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ -void Class::matlab_proxy(const string& classFile) { +void Class::matlab_proxy(const string& classFile) const { // open destination classFile ofstream ofs(classFile.c_str()); if(!ofs) throw CantOpenFile(classFile); if(verbose_) cerr << "generating " << classFile << endl; + // get the name of actual matlab object + string matlabName = qualifiedName(); + // emit class proxy code - ofs << "classdef " << name << endl; + ofs << "classdef " << matlabName << endl; ofs << " properties" << endl; ofs << " self = 0" << endl; ofs << " end" << endl; ofs << " methods" << endl; - ofs << " function obj = " << name << "(varargin)" << endl; + ofs << " function obj = " << matlabName << "(varargin)" << endl; BOOST_FOREACH(Constructor c, constructors) - c.matlab_proxy_fragment(ofs,name); - ofs << " if nargin ~= 13 && obj.self == 0, error('" << name << " constructor failed'); end" << endl; + c.matlab_proxy_fragment(ofs,matlabName); + ofs << " if nargin ~= 13 && obj.self == 0, error('" << matlabName << " constructor failed'); end" << endl; ofs << " end" << endl; ofs << " function display(obj), obj.print(''); end" << endl; ofs << " function disp(obj), obj.display; end" << endl; @@ -52,33 +57,98 @@ void Class::matlab_proxy(const string& classFile) { } /* ************************************************************************* */ -void Class::matlab_constructors(const string& toolboxPath,const string& nameSpace) { +void Class::matlab_constructors(const string& toolboxPath, const vector& using_namespaces) const { BOOST_FOREACH(Constructor c, constructors) { - c.matlab_mfile (toolboxPath, name); - c.matlab_wrapper(toolboxPath, name, nameSpace); + c.matlab_mfile (toolboxPath, qualifiedName()); + c.matlab_wrapper(toolboxPath, qualifiedName("::"), qualifiedName(), using_namespaces, includes); } } /* ************************************************************************* */ -void Class::matlab_methods(const string& classPath, const string& nameSpace) { +void Class::matlab_methods(const string& classPath, const vector& using_namespaces) const { + string matlabName = qualifiedName(), cppName = qualifiedName("::"); BOOST_FOREACH(Method m, methods) { m.matlab_mfile (classPath); - m.matlab_wrapper(classPath, name, nameSpace); + m.matlab_wrapper(classPath, name, cppName, matlabName, using_namespaces, includes); + } +} + +/* ************************************************************************* */ +void Class::matlab_static_methods(const string& toolboxPath, const vector& using_namespaces) const { + string matlabName = qualifiedName(), cppName = qualifiedName("::"); + BOOST_FOREACH(const StaticMethod& m, static_methods) { + m.matlab_mfile (toolboxPath, qualifiedName()); + m.matlab_wrapper(toolboxPath, name, matlabName, cppName, using_namespaces, includes); } } /* ************************************************************************* */ void Class::matlab_make_fragment(ofstream& ofs, const string& toolboxPath, - const string& mexFlags) -{ + const string& mexFlags) const { string mex = "mex " + mexFlags + " "; + string matlabClassName = qualifiedName(); BOOST_FOREACH(Constructor c, constructors) - ofs << mex << c.matlab_wrapper_name(name) << ".cpp" << endl; - ofs << endl << "cd @" << name << endl; + ofs << mex << c.matlab_wrapper_name(matlabClassName) << ".cpp" << endl; + BOOST_FOREACH(StaticMethod sm, static_methods) + ofs << mex << matlabClassName + "_" + sm.name << ".cpp" << endl; + ofs << endl << "cd @" << matlabClassName << endl; BOOST_FOREACH(Method m, methods) - ofs << mex << m.name_ << ".cpp" << endl; + ofs << mex << m.name << ".cpp" << endl; ofs << endl; } /* ************************************************************************* */ +void Class::makefile_fragment(ofstream& ofs) const { +// new_Point2_.$(MEXENDING): new_Point2_.cpp +// $(MEX) $(mex_flags) new_Point2_.cpp +// new_Point2_dd.$(MEXENDING): new_Point2_dd.cpp +// $(MEX) $(mex_flags) new_Point2_dd.cpp +// @Point2/x.$(MEXENDING): @Point2/x.cpp +// $(MEX) $(mex_flags) @Point2/x.cpp -output @Point2/x +// @Point2/y.$(MEXENDING): @Point2/y.cpp +// $(MEX) $(mex_flags) @Point2/y.cpp -output @Point2/y +// @Point2/dim.$(MEXENDING): @Point2/dim.cpp +// $(MEX) $(mex_flags) @Point2/dim.cpp -output @Point2/dim +// +// Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING) + + 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); + } + BOOST_FOREACH(StaticMethod c, static_methods) { + string file_base = matlabName + "_" + c.name; + file_names.push_back(file_base); + } + BOOST_FOREACH(Method c, methods) { + string file_base = "@" + matlabName + "/" + c.name; + file_names.push_back(file_base); + } + + BOOST_FOREACH(const string& file_base, file_names) { + ofs << file_base << ".$(MEXENDING): " << file_base << ".cpp" << endl; + ofs << "\t$(MEX) $(mex_flags) " << file_base << ".cpp -output " << file_base << endl; + } + + // class target + ofs << "\n" << matlabName << ": "; + BOOST_FOREACH(const string& file_base, file_names) { + ofs << file_base << ".$(MEXENDING) "; + } + ofs << "\n" << endl; +} + +/* ************************************************************************* */ +string Class::qualifiedName(const string& delim) const { + string result; + BOOST_FOREACH(const string& ns, namespaces) + result += ns + delim; + return result + name; +} + +/* ************************************************************************* */ diff --git a/wrap/Class.h b/wrap/Class.h index ca85b06b1..d8ebe1a10 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -18,10 +18,12 @@ #pragma once #include -#include #include "Constructor.h" #include "Method.h" +#include "StaticMethod.h" + +namespace wrap { /// Class has name, constructors, methods struct Class { @@ -29,19 +31,28 @@ struct Class { Class(bool verbose=true) : verbose_(verbose) {} // Then the instance variables are set directly by the Module constructor - std::string name; ///< Class name - std::list constructors; ///< Class constructors - std::list methods; ///< Class methods - bool verbose_; ///< verbose flag + 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 includes; ///< header include overrides + bool verbose_; ///< verbose flag // And finally MATLAB code is emitted, methods below called by Module::matlab_code - void matlab_proxy(const std::string& classFile); ///< emit proxy class + void matlab_proxy(const std::string& classFile) const; ///< emit proxy class void matlab_constructors(const std::string& toolboxPath, - const std::string& nameSpace); ///< emit constructor wrappers + const std::vector& using_namespaces) const; ///< emit constructor wrappers void matlab_methods(const std::string& classPath, - const std::string& nameSpace); ///< emit method wrappers + const std::vector& using_namespaces) const; ///< emit method wrappers + void matlab_static_methods(const std::string& classPath, + const std::vector& using_namespaces) const; ///< emit static method wrappers void matlab_make_fragment(std::ofstream& ofs, const std::string& toolboxPath, - const std::string& mexFlags); ///< emit make fragment for global make script + const std::string& mexFlags) const; ///< emit make fragment for global make script + void makefile_fragment(std::ofstream& ofs) const; ///< emit makefile fragment + std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter }; +} // \namespace wrap + diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 9caf4793f..a822f722f 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -23,15 +23,16 @@ #include "Constructor.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ -string Constructor::matlab_wrapper_name(const string& className) { +string Constructor::matlab_wrapper_name(const string& className) const { string str = "new_" + className + "_" + args.signature(); return str; } /* ************************************************************************* */ -void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) { +void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) const { ofs << " if nargin == " << args.size() << ", obj.self = " << matlab_wrapper_name(className) << "("; bool first = true; @@ -44,22 +45,22 @@ void Constructor::matlab_proxy_fragment(ofstream& ofs, const string& className) } /* ************************************************************************* */ -void Constructor::matlab_mfile(const string& toolboxPath, const string& className) { +void Constructor::matlab_mfile(const string& toolboxPath, const string& qualifiedMatlabName) const { - string name = matlab_wrapper_name(className); + string matlabName = matlab_wrapper_name(qualifiedMatlabName); // open destination m-file - string wrapperFile = toolboxPath + "/" + name + ".m"; + string wrapperFile = toolboxPath + "/" + matlabName + ".m"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - emit_header_comment(ofs, "%"); - ofs << "function result = " << name << "(obj"; + generateHeaderComment(ofs, "%"); + ofs << "function result = " << matlabName << "(obj"; if (args.size()) ofs << "," << args.names(); ofs << ")" << endl; - ofs << " error('need to compile " << name << ".cpp');" << endl; + ofs << " error('need to compile " << matlabName << ".cpp');" << endl; ofs << "end" << endl; // close file @@ -67,30 +68,29 @@ void Constructor::matlab_mfile(const string& toolboxPath, const string& classNam } /* ************************************************************************* */ -void Constructor::matlab_wrapper(const string& toolboxPath, - const string& className, - const string& nameSpace) -{ - - string name = matlab_wrapper_name(className); +void Constructor::matlab_wrapper(const string& toolboxPath, + const string& cppClassName, + const string& matlabClassName, + const vector& using_namespaces, const vector& includes) const { + string matlabName = matlab_wrapper_name(matlabClassName); // open destination wrapperFile - string wrapperFile = toolboxPath + "/" + name + ".cpp"; + string wrapperFile = toolboxPath + "/" + matlabName + ".cpp"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - emit_header_comment(ofs, "//"); - ofs << "#include " << endl; - ofs << "#include <" << className << ".h>" << endl; - if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; + generateHeaderComment(ofs, "//"); + generateIncludes(ofs, name, includes); + generateUsingNamespace(ofs, using_namespaces); + ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl; ofs << "{" << endl; - ofs << " checkArguments(\"" << name << "\",nargout,nargin," << args.size() << ");" << endl; + ofs << " checkArguments(\"" << matlabName << "\",nargout,nargin," << args.size() << ");" << endl; args.matlab_unwrap(ofs); // unwrap arguments - ofs << " " << className << "* self = new " << className << "(" << args.names() << ");" << endl; - ofs << " out[0] = wrap_constructed(self,\"" << className << "\");" << endl; + ofs << " " << cppClassName << "* self = new " << cppClassName << "(" << args.names() << ");" << endl; // need qualified name, delim: "::" + ofs << " out[0] = wrap_constructed(self,\"" << matlabClassName << "\");" << endl; // need matlab qualified name ofs << "}" << endl; // close file diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 340ac0fd2..b9a44e2cc 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -18,10 +18,12 @@ #pragma once #include -#include +#include #include "Argument.h" +namespace wrap { + // Constructor class struct Constructor { @@ -32,6 +34,7 @@ struct Constructor { // Then the instance variables are set directly by the Module constructor ArgumentList args; + std::string name; bool verbose_; // MATLAB code generation @@ -39,17 +42,22 @@ struct Constructor { // classFile is class proxy file, e.g., ../matlab/@Point2/Point2.m /// wrapper name - std::string matlab_wrapper_name(const std::string& className); + std::string matlab_wrapper_name(const std::string& className) const; /// proxy class fragment - void matlab_proxy_fragment(std::ofstream& ofs, const std::string& className); + void matlab_proxy_fragment(std::ofstream& ofs, const std::string& className) const; /// m-file void matlab_mfile(const std::string& toolboxPath, - const std::string& className); + const std::string& qualifiedMatlabName) const; - /// wrapper + /// cpp wrapper void matlab_wrapper(const std::string& toolboxPath, - const std::string& className, const std::string& nameSpace); + const std::string& cppClassName, + const std::string& matlabClassName, + const std::vector& using_namespaces, + const std::vector& includes) const; }; +} // \namespace wrap + diff --git a/wrap/Makefile.am b/wrap/Makefile.am index 895830537..8b3e4f493 100644 --- a/wrap/Makefile.am +++ b/wrap/Makefile.am @@ -9,15 +9,22 @@ AM_DEFAULT_SOURCE_EXT = .cpp headers = sources = -check_PROGRAMS = +check_PROGRAMS = +noinst_PROGRAMS = +wrap_PROGRAMS = +wrapdir = $(pkgincludedir)/wrap # disable all of matlab toolbox build by default if ENABLE_BUILD_TOOLBOX # Build a library from the core sources -sources += utilities.cpp Argument.cpp Constructor.cpp Method.cpp Class.cpp Module.cpp -check_PROGRAMS += tests/testSpirit tests/testWrap -noinst_PROGRAMS = wrap +sources += utilities.cpp Argument.cpp ReturnValue.cpp Constructor.cpp Method.cpp StaticMethod.cpp Class.cpp Module.cpp +check_PROGRAMS += tests/testSpirit tests/testWrap +if ENABLE_INSTALL_WRAP +wrap_PROGRAMS += wrap +else +noinst_PROGRAMS += wrap +endif #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed @@ -25,7 +32,6 @@ noinst_PROGRAMS = wrap #---------------------------------------------------------------------------------------------------- # Only install the header necessary for wrap interfaces to build with mex headers += matlab.h -wrapdir = $(pkgincludedir)/wrap wrap_HEADERS = $(headers) noinst_LTLIBRARIES = libwrap.la libwrap_la_SOURCES = $(sources) @@ -46,20 +52,79 @@ LDADD = libwrap.la ../CppUnitLite/libCppUnitLite.a # rule to run executable with valgrind %.valgrind: % $(LDADD) valgrind ./$^ - + # generate local toolbox dir interfacePath = $(top_srcdir) moduleName = gtsam toolboxpath = ../toolbox -nameSpace = "gtsam" -mexFlags = "${BOOST_CPPFLAGS} -DUNSAFE_WRAP -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" -all: - ./wrap ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags} - -# install the headers and matlab toolbox -install-exec-hook: all - install -d ${toolbox}/gtsam && \ - cp -rf ../toolbox/* ${toolbox}/gtsam + +# Set flags to pass to mex +mexFlags = +if ENABLE_UNSAFE_WRAP +mexFlags += "${BOOST_CPPFLAGS} -DUNSAFE_WRAP -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" +else +mexFlags += "${BOOST_CPPFLAGS} -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" +endif + +# Find the extension for mex binaries +# this should be done with mexext with matlab +mexextension = +if LINUX +if IS_64BIT +mexextension += mexa64 +else +mexextension += mexglx +endif +else # Linux +if DARWIN +mexextension += mexmaci64 +else +mexextension += mex_bin +endif +endif # Linux + +all: generate_toolbox + +generate_toolbox: $(top_srcdir)/gtsam.h + ./wrap ${mexextension} ${interfacePath} ${moduleName} ${toolboxpath} ${mexFlags} + +source_mode = -m 644 + +wrap-install-matlab-toolbox: generate_toolbox + install -d ${toolbox}/gtsam + install ${source_mode} -c ../toolbox/*.m ${toolbox}/gtsam + install ${source_mode} -c ../toolbox/*.cpp ${toolbox}/gtsam + install ${source_mode} -c ../toolbox/Makefile ${toolbox}/gtsam + cp -ru ../toolbox/@* ${toolbox}/gtsam + +wrap-install-bin: wrap + install -d ${wrap} + install -c ./wrap ${wrap} + +wrap-install-matlab-tests: + install -d ${toolbox}/gtsam/tests + install ${source_mode} -c ../../tests/matlab/*.m ${toolbox}/gtsam/tests + +wrap-install-matlab-examples: + install -d ${toolbox}/gtsam/examples + install ${source_mode} -c ../../examples/matlab/*.m ${toolbox}/gtsam/examples + +wrap_install_targets = +wrap_install_targets += wrap-install-matlab-toolbox + +if ENABLE_INSTALL_WRAP +wrap_install_targets += wrap-install-bin +endif + +if ENABLE_INSTALL_MATLAB_TESTS +wrap_install_targets += wrap-install-matlab-tests +endif + +if ENABLE_INSTALL_MATLAB_EXAMPLES +wrap_install_targets += wrap-install-matlab-examples +endif + +install-exec-hook: ${wrap_install_targets} # clean local toolbox dir clean: diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 1e0cd134e..0d9ba98e2 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -23,44 +23,24 @@ #include "utilities.h" using namespace std; +using namespace wrap; /* ************************************************************************* */ -// auxiliary function to wrap an argument into a shared_ptr template -/* ************************************************************************* */ -string maybe_shared_ptr(bool add, const string& type) { - string str = add? "shared_ptr<" : ""; - str += type; - if (add) str += ">"; - return str; -} - -/* ************************************************************************* */ -string Method::return_type(bool add_ptr, pairing p) { - if (p==pair && returns_pair_) { - string str = "pair< " + - maybe_shared_ptr(add_ptr && returns_ptr_, returns_) + ", " + - maybe_shared_ptr(add_ptr && returns_ptr_, returns2_) + " >"; - return str; - } else - return maybe_shared_ptr(add_ptr && returns_ptr_, (p==arg2)? returns2_ : returns_); -} - -/* ************************************************************************* */ -void Method::matlab_mfile(const string& classPath) { +void Method::matlab_mfile(const string& classPath) const { // open destination m-file - string wrapperFile = classPath + "/" + name_ + ".m"; + string wrapperFile = classPath + "/" + name + ".m"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); if(verbose_) cerr << "generating " << wrapperFile << endl; // generate code - string returnType = returns_pair_? "[first,second]" : "result"; - ofs << "function " << returnType << " = " << name_ << "(obj"; - if (args_.size()) ofs << "," << args_.names(); + string returnType = returnVal.matlab_returnType(); + ofs << "function " << returnType << " = " << name << "(obj"; + if (args.size()) ofs << "," << args.names(); ofs << ")" << endl; - ofs << "% usage: obj." << name_ << "(" << args_.names() << ")" << endl; - ofs << " error('need to compile " << name_ << ".cpp');" << endl; + ofs << "% usage: obj." << name << "(" << args.names() << ")" << endl; + ofs << " error('need to compile " << name << ".cpp');" << endl; ofs << "end" << endl; // close file @@ -70,10 +50,11 @@ void Method::matlab_mfile(const string& classPath) { /* ************************************************************************* */ void Method::matlab_wrapper(const string& classPath, const string& className, - const string& nameSpace) -{ + const string& cppClassName, + const string& matlabClassName, + const vector& using_namespaces, const std::vector& includes) const { // open destination wrapperFile - string wrapperFile = classPath + "/" + name_ + ".cpp"; + string wrapperFile = classPath + "/" + name + ".cpp"; ofstream ofs(wrapperFile.c_str()); if(!ofs) throw CantOpenFile(wrapperFile); if(verbose_) cerr << "generating " << wrapperFile << endl; @@ -81,10 +62,9 @@ void Method::matlab_wrapper(const string& classPath, // generate code // header - emit_header_comment(ofs, "//"); - ofs << "#include \n"; - ofs << "#include <" << className << ".h>\n"; - if (!nameSpace.empty()) ofs << "using namespace " << nameSpace << ";" << endl; + generateHeaderComment(ofs, "//"); + generateIncludes(ofs, className, includes); + generateUsingNamespace(ofs, using_namespaces); // call ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; @@ -94,39 +74,26 @@ void Method::matlab_wrapper(const string& classPath, // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); - ofs << " checkArguments(\"" << name_ << "\",nargout,nargin-1," << args_.size() << ");\n"; + ofs << " checkArguments(\"" << name << "\",nargout,nargin-1," << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - ofs << " shared_ptr<" << className << "> self = unwrap_shared_ptr< " << className - << " >(in[0],\"" << className << "\");" << endl; + ofs << " shared_ptr<" << cppClassName << "> self = unwrap_shared_ptr< " << cppClassName + << " >(in[0],\"" << matlabClassName << "\");" << endl; // unwrap arguments, see Argument.cpp - args_.matlab_unwrap(ofs,1); + args.matlab_unwrap(ofs,1); // call method // example: bool result = self->return_field(t); ofs << " "; - if (returns_!="void") - ofs << return_type(true,pair) << " result = "; - ofs << "self->" << name_ << "(" << args_.names() << ");\n"; + if (returnVal.type1!="void") + ofs << returnVal.return_type(true,ReturnValue::pair) << " result = "; + ofs << "self->" << name << "(" << args.names() << ");\n"; // wrap result // example: out[0]=wrap(result); - if (returns_pair_) { - if (returns_ptr_) - ofs << " out[0] = wrap_shared_ptr(result.first,\"" << returns_ << "\");\n"; - else - ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n"; - if (returns_ptr2_) - ofs << " out[1] = wrap_shared_ptr(result.second,\"" << returns2_ << "\");\n"; - else - ofs << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n"; - } - else if (returns_ptr_) - ofs << " out[0] = wrap_shared_ptr(result,\"" << returns_ << "\");\n"; - else if (returns_!="void") - ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n"; + returnVal.wrap_result(ofs); // finish ofs << "}\n"; diff --git a/wrap/Method.h b/wrap/Method.h index 56679e67b..bcfac82e3 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -21,33 +21,35 @@ #include #include "Argument.h" +#include "ReturnValue.h" + +namespace wrap { /// Method class struct Method { /// Constructor creates empty object Method(bool verbose = true) : - returns_ptr_(false), returns_ptr2_(false), returns_pair_(false), verbose_( - verbose) { - } + verbose_(verbose) {} // Then the instance variables are set directly by the Module constructor - bool is_const_; - ArgumentList args_; - std::string returns_, returns2_, name_; - bool returns_ptr_, returns_ptr2_, returns_pair_; bool verbose_; - - enum pairing { - arg1, arg2, pair - }; - std::string return_type(bool add_ptr, pairing p); + bool is_const_; + std::string name; + ArgumentList args; + ReturnValue returnVal; // MATLAB code generation // classPath is class directory, e.g., ../matlab/@Point2 - void matlab_mfile(const std::string& classPath); ///< m-file + void matlab_mfile(const std::string& classPath) const; ///< m-file void matlab_wrapper(const std::string& classPath, - const std::string& className, const std::string& nameSpace); ///< wrapper + const std::string& className, + const std::string& cppClassName, + const std::string& matlabClassname, + const std::vector& using_namespaces, + const std::vector& includes) const; ///< cpp wrapper }; +} // \namespace wrap + diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 7e35b0d2b..120b89f10 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -16,15 +16,19 @@ #include "Module.h" #include "utilities.h" +#include "pop_actor.h" //#define BOOST_SPIRIT_DEBUG #include +#include +#include #include #include #include using namespace std; +using namespace wrap; using namespace BOOST_SPIRIT_CLASSIC_NS; typedef rule Rule; @@ -38,102 +42,154 @@ typedef rule Rule; /* ************************************************************************* */ Module::Module(const string& interfacePath, - const string& moduleName, bool verbose) : name(moduleName), verbose_(verbose) + const string& moduleName, bool enable_verbose) : name(moduleName), verbose(enable_verbose) { // these variables will be imperatively updated to gradually build [cls] // The one with postfix 0 are used to reset the variables after parse. + ReturnValue retVal0, retVal; Argument arg0, arg; ArgumentList args0, args; - Constructor constructor0(verbose), constructor(verbose); - Method method0(verbose), method(verbose); - Class cls0(verbose),cls(verbose); + Constructor constructor0(enable_verbose), constructor(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); + vector namespaces, namespaces_return; //---------------------------------------------------------------------------- // Grammar with actions that build the Class object. Actions are // defined within the square brackets [] and are executed whenever a // rule is successfully parsed. Define BOOST_SPIRIT_DEBUG to debug. - // The grammar is allows a very restricted C++ header: - // - No comments allowed. - // -Only types allowed are string, bool, size_t int, double, Vector, and Matrix - // as well as class names that start with an uppercase letter - // - The types unsigned int and bool should be specified as int. + // The grammar is allows a very restricted C++ header // ---------------------------------------------------------------------------- + Rule comments_p = comment_p("/*", "*/") | comment_p("//", eol_p); + // lexeme_d turns off white space skipping // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html - Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')]; + Rule basisType_p = + (str_p("string") | "bool" | "size_t" | "int" | "double"); + + Rule keywords_p = + (str_p("const") | "static" | "namespace" | basisType_p); + + Rule eigenType_p = + (str_p("Vector") | "Matrix"); + + Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p; + + Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p; + + Rule namespace_arg_p = namespace_name_p[push_back_a(arg.namespaces)] >> str_p("::"); Rule classPtr_p = + *namespace_arg_p >> className_p [assign_a(arg.type)] >> ch_p('*') [assign_a(arg.is_ptr,true)]; Rule classRef_p = !str_p("const") [assign_a(arg.is_const,true)] >> + *namespace_arg_p >> className_p [assign_a(arg.type)] >> ch_p('&') [assign_a(arg.is_ref,true)]; - Rule basisType_p = - (str_p("string") | "bool" | "size_t" | "int" | "double"); + Rule argEigenType_p = + eigenType_p[assign_a(arg.type)] >> + !ch_p('*')[assign_a(arg.is_ptr,true)]; - Rule ublasType = - (str_p("Vector") | "Matrix")[assign_a(arg.type)] >> - !ch_p('*')[assign_a(arg.is_ptr,true)]; + Rule eigenRef_p = + !str_p("const") [assign_a(arg.is_const,true)] >> + eigenType_p [assign_a(arg.type)] >> + ch_p('&') [assign_a(arg.is_ref,true)]; Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; Rule argument_p = - ((basisType_p[assign_a(arg.type)] | ublasType | classPtr_p | classRef_p) >> name_p[assign_a(arg.name)]) + ((basisType_p[assign_a(arg.type)] | argEigenType_p | classRef_p | eigenRef_p | classPtr_p) + >> name_p[assign_a(arg.name)]) [push_back_a(args, arg)] [assign_a(arg,arg0)]; Rule argumentList_p = !argument_p >> * (',' >> argument_p); Rule constructor_p = - (className_p >> '(' >> argumentList_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)]; + Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::"); + Rule returnType1_p = - basisType_p[assign_a(method.returns_)] | - ((className_p | "Vector" | "Matrix")[assign_a(method.returns_)] >> - !ch_p('*') [assign_a(method.returns_ptr_,true)]); + (basisType_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::BASIS)]) | + ((*namespace_ret_p)[assign_a(retVal.namespaces1, namespaces_return)][clear_a(namespaces_return)] + >> (className_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::CLASS)]) >> + !ch_p('*')[assign_a(retVal.isPtr1,true)]) | + (eigenType_p[assign_a(retVal.type1)][assign_a(retVal.category1, ReturnValue::EIGEN)]); Rule returnType2_p = - basisType_p[assign_a(method.returns2_)] | - ((className_p | "Vector" | "Matrix")[assign_a(method.returns2_)] >> - !ch_p('*') [assign_a(method.returns_ptr2_,true)]); + (basisType_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::BASIS)]) | + ((*namespace_ret_p)[assign_a(retVal.namespaces2, namespaces_return)][clear_a(namespaces_return)] + >> (className_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::CLASS)]) >> + !ch_p('*') [assign_a(retVal.isPtr2,true)]) | + (eigenType_p[assign_a(retVal.type2)][assign_a(retVal.category2, ReturnValue::EIGEN)]); Rule pair_p = (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>') - [assign_a(method.returns_pair_,true)]; + [assign_a(retVal.isPair,true)]; - Rule void_p = str_p("void")[assign_a(method.returns_)]; + Rule void_p = str_p("void")[assign_a(retVal.type1)]; Rule returnType_p = void_p | returnType1_p | pair_p; Rule methodName_p = lexeme_d[lower_p >> *(alnum_p | '_')]; Rule method_p = - (returnType_p >> methodName_p[assign_a(method.name_)] >> + (returnType_p >> methodName_p[assign_a(method.name)] >> '(' >> argumentList_p >> ')' >> - !str_p("const")[assign_a(method.is_const_,true)] >> ';') - [assign_a(method.args_,args)] + !str_p("const")[assign_a(method.is_const_,true)] >> ';' >> *comments_p) + [assign_a(method.args,args)] [assign_a(args,args0)] + [assign_a(method.returnVal,retVal)] + [assign_a(retVal,retVal0)] [push_back_a(cls.methods, method)] [assign_a(method,method0)]; - Rule class_p = str_p("class") >> className_p[assign_a(cls.name)] >> '{' >> - *constructor_p >> - *method_p >> - '}' >> ";"; + Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; - Rule module_p = +class_p - [push_back_a(classes,cls)] - [assign_a(cls,cls0)] - >> !end_p; + Rule static_method_p = + (str_p("static") >> returnType_p >> staticMethodName_p[assign_a(static_method.name)] >> + '(' >> argumentList_p >> ')' >> ';' >> *comments_p) + [assign_a(static_method.args,args)] + [assign_a(args,args0)] + [assign_a(static_method.returnVal,retVal)] + [assign_a(retVal,retVal0)] + [push_back_a(cls.static_methods, static_method)] + [assign_a(static_method,static_method0)]; + + Rule includes_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(cls.includes)] >> ch_p('>'); + + Rule functions_p = includes_p | constructor_p | method_p | static_method_p; + + Rule class_p = (str_p("class") >> className_p[assign_a(cls.name)] >> '{' >> + *(functions_p | comments_p) >> + str_p("};"))[assign_a(cls.namespaces, namespaces)][push_back_a(classes,cls)][assign_a(cls,cls0)]; + + Rule namespace_def_p = str_p("namespace") >> + namespace_name_p[push_back_a(namespaces)] + >> ch_p('{') >> + *(class_p | namespace_def_p | comments_p) >> + str_p("}///\\namespace") >> !namespace_name_p // end namespace, avoid confusion with classes + [pop_a(namespaces)]; + + Rule using_namespace_p = str_p("using") >> str_p("namespace") + >> namespace_name_p[push_back_a(using_namespaces)] >> ch_p(';'); + + Rule module_content_p = comments_p | using_namespace_p | class_p | namespace_def_p ; + + Rule module_p = *module_content_p >> !end_p; //---------------------------------------------------------------------------- // for debugging, define BOOST_SPIRIT_DEBUG @@ -162,34 +218,45 @@ Module::Module(const string& interfacePath, string interfaceFile = interfacePath + "/" + moduleName + ".h"; string contents = file_contents(interfaceFile); - // Comment parser : does not work for some reason - rule<> comment_p = str_p("/*") >> +anychar_p >> "*/"; - rule<> skip_p = space_p; // | comment_p; - // and parse contents - parse_info info = parse(contents.c_str(), module_p, skip_p); + parse_info info = parse(contents.c_str(), module_p, space_p); if(!info.full) { printf("parsing stopped at \n%.20s\n",info.stop); throw ParseFailed(info.length); } } +/* ************************************************************************* */ +template +void verifyArguments(const vector& validArgs, const vector& vt) { + BOOST_FOREACH(const T& t, vt) { + BOOST_FOREACH(Argument arg, t.args) { + string fullType = arg.qualifiedType("::"); + if(find(validArgs.begin(), validArgs.end(), fullType) + == validArgs.end()) + throw DependencyMissing(fullType, t.name); + } + } +} + /* ************************************************************************* */ void Module::matlab_code(const string& toolboxPath, - const string& nameSpace, - const string& mexFlags) -{ - try { + const string& mexExt, const string& mexFlags) const { string installCmd = "install -d " + toolboxPath; system(installCmd.c_str()); // create make m-file - string makeFile = toolboxPath + "/make_" + name + ".m"; - ofstream ofs(makeFile.c_str()); - if(!ofs) throw CantOpenFile(makeFile); + string matlabMakeFile = toolboxPath + "/make_" + name + ".m"; + ofstream ofs(matlabMakeFile.c_str()); + if(!ofs) throw CantOpenFile(matlabMakeFile); - if (verbose_) cerr << "generating " << makeFile << endl; - emit_header_comment(ofs,"%"); + // create the (actual) make file + string makeFile = toolboxPath + "/Makefile"; + ofstream make_ofs(makeFile.c_str()); + if(!make_ofs) throw CantOpenFile(makeFile); + + if (verbose) cerr << "generating " << matlabMakeFile << endl; + generateHeaderComment(ofs,"%"); ofs << "echo on" << endl << endl; ofs << "toolboxpath = mfilename('fullpath');" << endl; ofs << "delims = find(toolboxpath == '/');" << endl; @@ -197,36 +264,76 @@ void Module::matlab_code(const string& toolboxPath, ofs << "clear delims" << endl; ofs << "addpath(toolboxpath);" << endl << endl; + if (verbose) cerr << "generating " << makeFile << endl; + generateHeaderComment(make_ofs,"#"); + make_ofs << "\nMEX = mex\n"; + make_ofs << "MEXENDING = " << mexExt << "\n"; + make_ofs << "mex_flags = " << mexFlags << "\n\n"; + + //Dependency check list + vector validArgs; + validArgs.push_back("string"); + validArgs.push_back("int"); + validArgs.push_back("bool"); + validArgs.push_back("size_t"); + validArgs.push_back("double"); + validArgs.push_back("Vector"); + validArgs.push_back("Matrix"); + + // add 'all' to Makefile + make_ofs << "all: "; + BOOST_FOREACH(Class cls, classes) { + make_ofs << cls.qualifiedName() << " "; + //Create a list of parsed classes for dependency checking + validArgs.push_back(cls.qualifiedName("::")); + } + make_ofs << "\n\n"; + // generate proxy classes and wrappers BOOST_FOREACH(Class cls, classes) { // create directory if needed - string classPath = toolboxPath + "/@" + cls.name; + string classPath = toolboxPath + "/@" + cls.qualifiedName(); string installCmd = "install -d " + classPath; system(installCmd.c_str()); // create proxy class - string classFile = classPath + "/" + cls.name + ".m"; + string classFile = classPath + "/" + cls.qualifiedName() + ".m"; cls.matlab_proxy(classFile); + // verify all of the function arguments + verifyArguments(validArgs, cls.constructors); + verifyArguments(validArgs, cls.static_methods); + verifyArguments(validArgs, cls.methods); + // create constructor and method wrappers - cls.matlab_constructors(toolboxPath,nameSpace); - cls.matlab_methods(classPath,nameSpace); + cls.matlab_constructors(toolboxPath,using_namespaces); + cls.matlab_static_methods(toolboxPath,using_namespaces); + cls.matlab_methods(classPath,using_namespaces); // add lines to make m-file - ofs << "%% " << cls.name << endl; + ofs << "%% " << cls.qualifiedName() << endl; ofs << "cd(toolboxpath)" << endl; cls.matlab_make_fragment(ofs, toolboxPath, mexFlags); + + // add section to the (actual) make file + make_ofs << "# " << cls.qualifiedName() << endl; + cls.makefile_fragment(make_ofs); } // finish make m-file ofs << "cd(toolboxpath)" << endl << endl; ofs << "echo off" << endl; ofs.close(); - } - catch(exception &e) { - cerr << "generate_matlab_toolbox failed because " << e.what() << endl; - } -} + // make clean at end of Makefile + make_ofs << "\n\nclean: \n"; + make_ofs << "\trm -rf *.$(MEXENDING)\n"; + BOOST_FOREACH(Class cls, classes) + make_ofs << "\trm -rf @" << cls.qualifiedName() << "/*.$(MEXENDING)\n"; + + // finish Makefile + make_ofs << "\n" << endl; + make_ofs.close(); + } /* ************************************************************************* */ diff --git a/wrap/Module.h b/wrap/Module.h index 0227e86d9..ec057b644 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -18,26 +18,30 @@ #pragma once #include -#include +#include #include "Class.h" +namespace wrap { + /** * A module just has a name and a list of classes */ struct Module { std::string name; ///< module name - std::list classes; ///< list of classes - bool verbose_; ///< verbose flag + std::vector classes; ///< list of classes + bool verbose; ///< verbose flag + std::vector using_namespaces; ///< all default namespaces /// constructor that parses interface file - Module(const std::string& interfacePath, + Module(const std::string& interfacePath, const std::string& moduleName, - bool verbose=true); + bool enable_verbose=true); /// MATLAB code generation: void matlab_code(const std::string& path, - const std::string& nameSpace, - const std::string& mexFlags); + const std::string& mexExt, + const std::string& mexFlags) const; }; +} // \namespace wrap diff --git a/wrap/README b/wrap/README index 6996e3649..c47b34ddd 100644 --- a/wrap/README +++ b/wrap/README @@ -17,10 +17,10 @@ OBJECT CREATION new_GaussianFactorGraph_ calls wrap_constructed in matlab.h, see documentation there METHOD (AND CONSTRUCTOR) ARGUMENTS -- simple argument types of methods, such as "double", will be converted in the +- Simple argument types of methods, such as "double", will be converted in the mex warppers by calling unwrap, defined in matlab.h -- Vector and Matric arguments are normally passed by reference in GTSAM, but - in gtsam.h you need to pretedn they are passed by value, to trigger the +- Vector and Matrix arguments are normally passed by reference in GTSAM, but + in gtsam.h you need to pretend they are passed by value, to trigger the generation of the correct conversion routines unwrap and unwrap - passing classes as arguments works, provided they are passed by reference. This triggers a call to unwrap_shared_ptr diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp new file mode 100644 index 000000000..38d85d074 --- /dev/null +++ b/wrap/ReturnValue.cpp @@ -0,0 +1,78 @@ +/** + * @file ReturnValue.cpp + * + * @date Dec 1, 2011 + * @author Alex Cunningham + */ + +#include + +#include "ReturnValue.h" +#include "utilities.h" + +using namespace std; +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("::")) + " >"; + return str; + } else + return maybe_shared_ptr(add_ptr && isPtr1, (p==arg2)? qualifiedType2("::") : qualifiedType1("::")); +} + +/* ************************************************************************* */ +string ReturnValue::matlab_returnType() const { + return isPair? "[first,second]" : "result"; +} + +/* ************************************************************************* */ +string ReturnValue::qualifiedType1(const string& delim) const { + string result; + BOOST_FOREACH(const string& ns, namespaces1) result += ns + delim; + return result + type1; +} + +/* ************************************************************************* */ +string ReturnValue::qualifiedType2(const string& delim) const { + string result; + BOOST_FOREACH(const string& ns, namespaces2) result += ns + delim; + return result + type2; +} + +/* ************************************************************************* */ +void ReturnValue::wrap_result(ostream& ofs) 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 + ofs << " out[0] = wrap_shared_ptr(result.first,\"" << matlabType1 << "\");\n"; + else if (category1 == ReturnValue::CLASS) // if we are going to make one + ofs << " out[0] = wrap_shared_ptr(make_shared< " << cppType1 << " >(result.first),\"" << matlabType1 << "\");\n"; + else // if basis type + ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n"; + + // second return value in pair + if (isPtr2) // if we already have a pointer + ofs << " out[1] = wrap_shared_ptr(result.second,\"" << type2 << "\");\n"; + else if (category2 == ReturnValue::CLASS) // if we are going to make one + ofs << " out[1] = wrap_shared_ptr(make_shared< " << cppType2 << " >(result.second),\"" << matlabType2 << "\");\n"; + else + ofs << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n"; + } + else if (isPtr1) + ofs << " out[0] = wrap_shared_ptr(result,\"" << type1 << "\");\n"; + else if (category1 == ReturnValue::CLASS) + ofs << " out[0] = wrap_shared_ptr(make_shared< " << cppType1 << " >(result),\"" << matlabType1 << "\");\n"; + else if (type1!="void") + ofs << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n"; +} + +/* ************************************************************************* */ + + diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h new file mode 100644 index 000000000..eb2406d80 --- /dev/null +++ b/wrap/ReturnValue.h @@ -0,0 +1,53 @@ +/** + * @file ReturnValue.h + * + * @brief Encapsulates a return value from a method + * + * @date Dec 1, 2011 + * @author Alex Cunningham + */ + +#include +#include + +#pragma once + +namespace wrap { + +struct ReturnValue { + + typedef enum { + CLASS, + EIGEN, + BASIS, + VOID + } return_category; + + ReturnValue(bool enable_verbosity = true) + : verbose(enable_verbosity), isPtr1(false), isPtr2(false), + isPair(false), category1(VOID), category2(VOID) + {} + + bool verbose; + std::string type1, type2; + bool isPtr1, isPtr2, isPair; + std::vector namespaces1, namespaces2; + + return_category category1, category2; + + typedef enum { + arg1, arg2, pair + } pairing; + + std::string return_type(bool add_ptr, pairing p) const; + + std::string qualifiedType1(const std::string& delim = "") const; + std::string qualifiedType2(const std::string& delim = "") const; + + std::string matlab_returnType() const; + + void wrap_result(std::ostream& ofs) const; + +}; + +} // \namespace wrap diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp new file mode 100644 index 000000000..ff4d31592 --- /dev/null +++ b/wrap/StaticMethod.cpp @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * 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 Method.ccp + * @author Frank Dellaert + **/ + +#include +#include + +#include + +#include "StaticMethod.h" +#include "utilities.h" + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +void StaticMethod::matlab_mfile(const string& toolboxPath, const string& className) const { + + // open destination m-file + string full_name = className + "_" + name; + string wrapperFile = toolboxPath + "/" + full_name + ".m"; + ofstream ofs(wrapperFile.c_str()); + if(!ofs) throw CantOpenFile(wrapperFile); + if(verbose) cerr << "generating " << wrapperFile << endl; + + // generate code + string returnType = returnVal.matlab_returnType(); + ofs << "function " << returnType << " = " << full_name << "("; + if (args.size()) ofs << args.names(); + ofs << ")" << endl; + ofs << "% usage: x = " << full_name << "(" << args.names() << ")" << endl; + ofs << " error('need to compile " << full_name << ".cpp');" << endl; + ofs << "end" << endl; + + // close file + ofs.close(); +} + +/* ************************************************************************* */ +void StaticMethod::matlab_wrapper(const string& toolboxPath, const string& className, + const string& matlabClassName, const string& cppClassName, + const vector& using_namespaces, + const vector& includes) const { + // open destination wrapperFile + string full_name = matlabClassName + "_" + name; + string wrapperFile = toolboxPath + "/" + full_name + ".cpp"; + ofstream ofs(wrapperFile.c_str()); + if(!ofs) throw CantOpenFile(wrapperFile); + if(verbose) cerr << "generating " << wrapperFile << endl; + + // generate code + + // header + generateHeaderComment(ofs, "//"); + generateIncludes(ofs, className, includes); + generateUsingNamespace(ofs, using_namespaces); + + // call + ofs << "void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + // start + ofs << "{\n"; + + // check arguments + // NOTE: for static functions, there is no object passed + ofs << " checkArguments(\"" << full_name << "\",nargout,nargin," << args.size() << ");\n"; + + // unwrap arguments, see Argument.cpp + args.matlab_unwrap(ofs,0); // We start at 0 because there is no self object + + ofs << " "; + + // call method with default type + if (returnVal.type1!="void") + ofs << returnVal.return_type(true,ReturnValue::pair) << " result = "; + ofs << cppClassName << "::" << name << "(" << args.names() << ");\n"; + + // wrap result + // example: out[0]=wrap(result); + returnVal.wrap_result(ofs); + + // finish + ofs << "}\n"; + + // close file + ofs.close(); +} + +/* ************************************************************************* */ diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h new file mode 100644 index 000000000..f9cc14af7 --- /dev/null +++ b/wrap/StaticMethod.h @@ -0,0 +1,56 @@ +/* ---------------------------------------------------------------------------- + + * 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 StaticMethod.h + * @brief describes and generates code for static methods + * @author Frank Dellaert + * @author Alex Cunningham + **/ + +#pragma once + +#include +#include + +#include "Argument.h" +#include "ReturnValue.h" + +namespace wrap { + +/// StaticMethod class +struct StaticMethod { + + /// Constructor creates empty object + StaticMethod(bool verbosity = true) : + verbose(verbosity) {} + + // Then the instance variables are set directly by the Module constructor + bool verbose; + std::string name; + ArgumentList args; + ReturnValue returnVal; + + // MATLAB code generation + // toolboxPath is the core toolbox directory, e.g., ../matlab + // NOTE: static functions are not inside the class, and + // are created with [ClassName]_[FunctionName]() format + + void matlab_mfile(const std::string& toolboxPath, const std::string& className) const; ///< m-file + void matlab_wrapper(const std::string& toolboxPath, + const std::string& className, const std::string& matlabClassName, + const std::string& cppClassName, + const std::vector& using_namespaces, + const std::vector& includes) const; ///< cpp wrapper +}; + +} // \namespace wrap + diff --git a/wrap/geometry.h b/wrap/geometry.h deleted file mode 100644 index 9b1d80b00..000000000 --- a/wrap/geometry.h +++ /dev/null @@ -1,39 +0,0 @@ - -class Point2 { - Point2(); - Point2(double x, double y); - double x(); - double y(); - int dim() const; -}; - -class Point3 { - Point3(double x, double y, double z); - double norm() const; -}; - -class Test { - Test(); - - bool return_bool (bool value); - size_t return_size_t (size_t value); - int return_int (int value); - double return_double (double value); - - string return_string (string value); - Vector return_vector1(Vector value); - Matrix return_matrix1(Matrix value); - Vector return_vector2(Vector value); - Matrix return_matrix2(Matrix value); - - pair return_pair (Vector v, Matrix A); - - bool return_field(const Test& t) const; - - Test* return_TestPtr(Test* value); - - pair create_ptrs (); - pair return_ptrs (Test* p1, Test* p2); - - void print(); -}; diff --git a/wrap/matlab.h b/wrap/matlab.h index 410798718..add125cb5 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -27,6 +27,7 @@ extern "C" { } #include +#include #include #include @@ -36,9 +37,6 @@ using namespace std; using namespace boost; // not usual, but for conciseness of generated code // start GTSAM Specifics ///////////////////////////////////////////////// -//typedef gtsam::Vector Vector; // NOTE: outside of gtsam namespace -//typedef gtsam::Matrix Matrix; - // to make keys be constructed from strings: #define GTSAM_MAGIC_KEY // to enable Matrix and Vector constructor for SharedGaussian: diff --git a/wrap/pop_actor.h b/wrap/pop_actor.h new file mode 100644 index 000000000..97bfdcca6 --- /dev/null +++ b/wrap/pop_actor.h @@ -0,0 +1,59 @@ +/** + * @file pop_actor.h + * + * @brief An actor to pop a vector/container, based off of the clear_actor + * + * @date Dec 8, 2011 + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace boost { namespace spirit { + +BOOST_SPIRIT_CLASSIC_NAMESPACE_BEGIN + + /////////////////////////////////////////////////////////////////////////// + // Summary: + // A semantic action policy that calls pop_back method. + // (This doc uses convention available in actors.hpp) + // + // Actions (what it does): + // ref.pop_back(); + // + // Policy name: + // clear_action + // + // Policy holder, corresponding helper method: + // ref_actor, clear_a( ref ); + // + // () operators: both. + // + // See also ref_actor for more details. + /////////////////////////////////////////////////////////////////////////// + struct pop_action + { + template< + typename T + > + void act(T& ref_) const + { + ref_.pop_back(); + } + }; + + /////////////////////////////////////////////////////////////////////////// + // helper method that creates a and_assign_actor. + /////////////////////////////////////////////////////////////////////////// + template + inline ref_actor pop_a(T& ref_) + { + return ref_actor(ref_); + } + +BOOST_SPIRIT_CLASSIC_NAMESPACE_END + +}} + diff --git a/wrap/tests/expected/@Point2/dim.cpp b/wrap/tests/expected/@Point2/dim.cpp index f81c416fe..e845db94a 100644 --- a/wrap/tests/expected/@Point2/dim.cpp +++ b/wrap/tests/expected/@Point2/dim.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("dim",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Point2/dim.m b/wrap/tests/expected/@Point2/dim.m index afd9ee5f7..40715292e 100644 --- a/wrap/tests/expected/@Point2/dim.m +++ b/wrap/tests/expected/@Point2/dim.m @@ -1,5 +1,4 @@ function result = dim(obj) % usage: obj.dim() -% automatically generated by wrap on 2011-Oct-31 error('need to compile dim.cpp'); end diff --git a/wrap/tests/expected/@Point2/vectorConfusion.cpp b/wrap/tests/expected/@Point2/vectorConfusion.cpp new file mode 100644 index 000000000..cdb06b2df --- /dev/null +++ b/wrap/tests/expected/@Point2/vectorConfusion.cpp @@ -0,0 +1,11 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("vectorConfusion",nargout,nargin-1,0); + shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + VectorNotEigen result = self->vectorConfusion(); + out[0] = wrap_shared_ptr(make_shared< VectorNotEigen >(result),"VectorNotEigen"); +} diff --git a/wrap/tests/expected/@Point2/vectorConfusion.m b/wrap/tests/expected/@Point2/vectorConfusion.m new file mode 100644 index 000000000..d141e8085 --- /dev/null +++ b/wrap/tests/expected/@Point2/vectorConfusion.m @@ -0,0 +1,4 @@ +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 da5ed8a0c..d245748d0 100644 --- a/wrap/tests/expected/@Point2/x.cpp +++ b/wrap/tests/expected/@Point2/x.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("x",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Point2/x.m b/wrap/tests/expected/@Point2/x.m index 3309b286c..f91039ab7 100644 --- a/wrap/tests/expected/@Point2/x.m +++ b/wrap/tests/expected/@Point2/x.m @@ -1,5 +1,4 @@ function result = x(obj) % usage: obj.x() -% automatically generated by wrap on 2011-Oct-31 error('need to compile x.cpp'); end diff --git a/wrap/tests/expected/@Point2/y.cpp b/wrap/tests/expected/@Point2/y.cpp index 3d61a4048..6342d6238 100644 --- a/wrap/tests/expected/@Point2/y.cpp +++ b/wrap/tests/expected/@Point2/y.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("y",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Point2/y.m b/wrap/tests/expected/@Point2/y.m index 015a11144..f9f36ae90 100644 --- a/wrap/tests/expected/@Point2/y.m +++ b/wrap/tests/expected/@Point2/y.m @@ -1,5 +1,4 @@ function result = y(obj) % usage: obj.y() -% automatically generated by wrap on 2011-Oct-31 error('need to compile y.cpp'); end diff --git a/wrap/tests/expected/@Point3/norm.cpp b/wrap/tests/expected/@Point3/norm.cpp index 6daaadfd1..ad36e32b8 100644 --- a/wrap/tests/expected/@Point3/norm.cpp +++ b/wrap/tests/expected/@Point3/norm.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("norm",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Test/Test.m b/wrap/tests/expected/@Test/Test.m index 19b4442e4..8574a0882 100644 --- a/wrap/tests/expected/@Test/Test.m +++ b/wrap/tests/expected/@Test/Test.m @@ -5,6 +5,7 @@ classdef Test methods function obj = Test(varargin) if nargin == 0, obj.self = new_Test_(); end + if nargin == 2, obj.self = new_Test_dM(varargin{1},varargin{2}); end if nargin ~= 13 && obj.self == 0, error('Test constructor failed'); end end function display(obj), obj.print(''); end diff --git a/wrap/tests/expected/@Test/arg_EigenConstRef.cpp b/wrap/tests/expected/@Test/arg_EigenConstRef.cpp new file mode 100644 index 000000000..e44b74f1b --- /dev/null +++ b/wrap/tests/expected/@Test/arg_EigenConstRef.cpp @@ -0,0 +1,11 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("arg_EigenConstRef",nargout,nargin-1,1); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "Matrix"); + self->arg_EigenConstRef(value); +} diff --git a/wrap/tests/expected/@Test/arg_EigenConstRef.m b/wrap/tests/expected/@Test/arg_EigenConstRef.m new file mode 100644 index 000000000..e353faa29 --- /dev/null +++ b/wrap/tests/expected/@Test/arg_EigenConstRef.m @@ -0,0 +1,4 @@ +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 new file mode 100644 index 000000000..a2c237c05 --- /dev/null +++ b/wrap/tests/expected/@Test/create_MixedPtrs.cpp @@ -0,0 +1,12 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("create_MixedPtrs",nargout,nargin-1,0); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + pair< Test, shared_ptr > result = self->create_MixedPtrs(); + out[0] = wrap_shared_ptr(make_shared< Test >(result.first),"Test"); + out[1] = wrap_shared_ptr(result.second,"Test"); +} diff --git a/wrap/tests/expected/@Test/create_MixedPtrs.m b/wrap/tests/expected/@Test/create_MixedPtrs.m new file mode 100644 index 000000000..9062cabcb --- /dev/null +++ b/wrap/tests/expected/@Test/create_MixedPtrs.m @@ -0,0 +1,4 @@ +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 690466298..33722bd14 100644 --- a/wrap/tests/expected/@Test/create_ptrs.cpp +++ b/wrap/tests/expected/@Test/create_ptrs.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("create_ptrs",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Test/create_ptrs.m b/wrap/tests/expected/@Test/create_ptrs.m index 07325487f..11af0ac5b 100644 --- a/wrap/tests/expected/@Test/create_ptrs.m +++ b/wrap/tests/expected/@Test/create_ptrs.m @@ -1,5 +1,4 @@ function [first,second] = create_ptrs(obj) % usage: obj.create_ptrs() -% automatically generated by wrap on 2011-Oct-31 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 9c516c6b7..e92a58b10 100644 --- a/wrap/tests/expected/@Test/print.cpp +++ b/wrap/tests/expected/@Test/print.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); diff --git a/wrap/tests/expected/@Test/return_Point2Ptr.cpp b/wrap/tests/expected/@Test/return_Point2Ptr.cpp new file mode 100644 index 000000000..99cf67f0b --- /dev/null +++ b/wrap/tests/expected/@Test/return_Point2Ptr.cpp @@ -0,0 +1,12 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("return_Point2Ptr",nargout,nargin-1,1); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + bool value = unwrap< bool >(in[1]); + shared_ptr result = self->return_Point2Ptr(value); + out[0] = wrap_shared_ptr(result,"Point2"); +} diff --git a/wrap/tests/expected/@Test/return_Point2Ptr.m b/wrap/tests/expected/@Test/return_Point2Ptr.m new file mode 100644 index 000000000..2da8b3710 --- /dev/null +++ b/wrap/tests/expected/@Test/return_Point2Ptr.m @@ -0,0 +1,4 @@ +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 new file mode 100644 index 000000000..19256f9ac --- /dev/null +++ b/wrap/tests/expected/@Test/return_Test.cpp @@ -0,0 +1,12 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("return_Test",nargout,nargin-1,1); + shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + shared_ptr value = unwrap_shared_ptr< Test >(in[1], "Test"); + Test result = self->return_Test(value); + out[0] = wrap_shared_ptr(make_shared< Test >(result),"Test"); +} diff --git a/wrap/tests/expected/@Test/return_Test.m b/wrap/tests/expected/@Test/return_Test.m new file mode 100644 index 000000000..01f4ec61b --- /dev/null +++ b/wrap/tests/expected/@Test/return_Test.m @@ -0,0 +1,4 @@ +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 b4ebecfd8..39ed01f15 100644 --- a/wrap/tests/expected/@Test/return_TestPtr.cpp +++ b/wrap/tests/expected/@Test/return_TestPtr.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_TestPtr",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_TestPtr.m b/wrap/tests/expected/@Test/return_TestPtr.m index 5da63f9c1..e1d0c90f5 100644 --- a/wrap/tests/expected/@Test/return_TestPtr.m +++ b/wrap/tests/expected/@Test/return_TestPtr.m @@ -1,5 +1,4 @@ function result = return_TestPtr(obj,value) % usage: obj.return_TestPtr(value) -% automatically generated by wrap on 2011-Oct-31 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 5a797b963..016bf2934 100644 --- a/wrap/tests/expected/@Test/return_bool.cpp +++ b/wrap/tests/expected/@Test/return_bool.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_bool",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_bool.m b/wrap/tests/expected/@Test/return_bool.m index bf41045d3..185ab992d 100644 --- a/wrap/tests/expected/@Test/return_bool.m +++ b/wrap/tests/expected/@Test/return_bool.m @@ -1,5 +1,4 @@ function result = return_bool(obj,value) % usage: obj.return_bool(value) -% automatically generated by wrap on 2011-Oct-31 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 c1dc96f9d..c087c194b 100644 --- a/wrap/tests/expected/@Test/return_double.cpp +++ b/wrap/tests/expected/@Test/return_double.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_double",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_double.m b/wrap/tests/expected/@Test/return_double.m index e8c154440..a6ba733cf 100644 --- a/wrap/tests/expected/@Test/return_double.m +++ b/wrap/tests/expected/@Test/return_double.m @@ -1,5 +1,4 @@ function result = return_double(obj,value) % usage: obj.return_double(value) -% automatically generated by wrap on 2011-Oct-31 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 adcb76bfd..3c2ea29b0 100644 --- a/wrap/tests/expected/@Test/return_field.cpp +++ b/wrap/tests/expected/@Test/return_field.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_field",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_field.m b/wrap/tests/expected/@Test/return_field.m index d99196354..278ffa411 100644 --- a/wrap/tests/expected/@Test/return_field.m +++ b/wrap/tests/expected/@Test/return_field.m @@ -1,5 +1,4 @@ function result = return_field(obj,t) % usage: obj.return_field(t) -% automatically generated by wrap on 2011-Oct-31 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 fc24edc3c..c9bada69e 100644 --- a/wrap/tests/expected/@Test/return_int.cpp +++ b/wrap/tests/expected/@Test/return_int.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_int",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_int.m b/wrap/tests/expected/@Test/return_int.m index a36990b35..8b613285c 100644 --- a/wrap/tests/expected/@Test/return_int.m +++ b/wrap/tests/expected/@Test/return_int.m @@ -1,5 +1,4 @@ function result = return_int(obj,value) % usage: obj.return_int(value) -% automatically generated by wrap on 2011-Oct-31 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 07c59cebc..acd39c9f5 100644 --- a/wrap/tests/expected/@Test/return_matrix1.cpp +++ b/wrap/tests/expected/@Test/return_matrix1.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix1",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_matrix1.m b/wrap/tests/expected/@Test/return_matrix1.m index 87200044f..29743158c 100644 --- a/wrap/tests/expected/@Test/return_matrix1.m +++ b/wrap/tests/expected/@Test/return_matrix1.m @@ -1,5 +1,4 @@ function result = return_matrix1(obj,value) % usage: obj.return_matrix1(value) -% automatically generated by wrap on 2011-Oct-31 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 6a942b54d..50e2ee462 100644 --- a/wrap/tests/expected/@Test/return_matrix2.cpp +++ b/wrap/tests/expected/@Test/return_matrix2.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix2",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_matrix2.m b/wrap/tests/expected/@Test/return_matrix2.m index 05b9c9c9c..e9ec91678 100644 --- a/wrap/tests/expected/@Test/return_matrix2.m +++ b/wrap/tests/expected/@Test/return_matrix2.m @@ -1,5 +1,4 @@ function result = return_matrix2(obj,value) % usage: obj.return_matrix2(value) -% automatically generated by wrap on 2011-Oct-31 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 637f0b365..a10e79109 100644 --- a/wrap/tests/expected/@Test/return_pair.cpp +++ b/wrap/tests/expected/@Test/return_pair.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,2); diff --git a/wrap/tests/expected/@Test/return_pair.m b/wrap/tests/expected/@Test/return_pair.m index 61a0138f8..a97f7c46e 100644 --- a/wrap/tests/expected/@Test/return_pair.m +++ b/wrap/tests/expected/@Test/return_pair.m @@ -1,5 +1,4 @@ function [first,second] = return_pair(obj,v,A) % usage: obj.return_pair(v,A) -% automatically generated by wrap on 2011-Oct-31 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 ae1ceae84..6c2ab46a6 100644 --- a/wrap/tests/expected/@Test/return_ptrs.cpp +++ b/wrap/tests/expected/@Test/return_ptrs.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_ptrs",nargout,nargin-1,2); diff --git a/wrap/tests/expected/@Test/return_ptrs.m b/wrap/tests/expected/@Test/return_ptrs.m index 8eb7d0ce2..ef7f8e5fc 100644 --- a/wrap/tests/expected/@Test/return_ptrs.m +++ b/wrap/tests/expected/@Test/return_ptrs.m @@ -1,5 +1,4 @@ function [first,second] = return_ptrs(obj,p1,p2) % usage: obj.return_ptrs(p1,p2) -% automatically generated by wrap on 2011-Oct-31 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 1ffaf6a4f..af1524ec7 100644 --- a/wrap/tests/expected/@Test/return_size_t.cpp +++ b/wrap/tests/expected/@Test/return_size_t.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_size_t",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_size_t.m b/wrap/tests/expected/@Test/return_size_t.m index 9124824b7..bc2734410 100644 --- a/wrap/tests/expected/@Test/return_size_t.m +++ b/wrap/tests/expected/@Test/return_size_t.m @@ -1,5 +1,4 @@ function result = return_size_t(obj,value) % usage: obj.return_size_t(value) -% automatically generated by wrap on 2011-Oct-31 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 3055a8422..71a86e63b 100644 --- a/wrap/tests/expected/@Test/return_string.cpp +++ b/wrap/tests/expected/@Test/return_string.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_string",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_string.m b/wrap/tests/expected/@Test/return_string.m index ad22de567..3f2304a65 100644 --- a/wrap/tests/expected/@Test/return_string.m +++ b/wrap/tests/expected/@Test/return_string.m @@ -1,5 +1,4 @@ function result = return_string(obj,value) % usage: obj.return_string(value) -% automatically generated by wrap on 2011-Oct-31 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 e1a384957..df8779989 100644 --- a/wrap/tests/expected/@Test/return_vector1.cpp +++ b/wrap/tests/expected/@Test/return_vector1.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector1",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_vector1.m b/wrap/tests/expected/@Test/return_vector1.m index a0ee6e8ba..f67382978 100644 --- a/wrap/tests/expected/@Test/return_vector1.m +++ b/wrap/tests/expected/@Test/return_vector1.m @@ -1,5 +1,4 @@ function result = return_vector1(obj,value) % usage: obj.return_vector1(value) -% automatically generated by wrap on 2011-Oct-31 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 1380643c1..ac87ab83a 100644 --- a/wrap/tests/expected/@Test/return_vector2.cpp +++ b/wrap/tests/expected/@Test/return_vector2.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector2",nargout,nargin-1,1); diff --git a/wrap/tests/expected/@Test/return_vector2.m b/wrap/tests/expected/@Test/return_vector2.m index 3768e9e0e..95b6bcfd6 100644 --- a/wrap/tests/expected/@Test/return_vector2.m +++ b/wrap/tests/expected/@Test/return_vector2.m @@ -1,5 +1,4 @@ function result = return_vector2(obj,value) % usage: obj.return_vector2(value) -% automatically generated by wrap on 2011-Oct-31 error('need to compile return_vector2.cpp'); end diff --git a/wrap/tests/expected/Makefile b/wrap/tests/expected/Makefile new file mode 100644 index 000000000..03f69a496 --- /dev/null +++ b/wrap/tests/expected/Makefile @@ -0,0 +1,91 @@ +# automatically generated by wrap on 2011-Dec-06 + +MEX = mex +MEXENDING = mexa64 +mex_flags = -O5 + +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 +@Point2/x.$(MEXENDING): @Point2/x.cpp + $(MEX) $(mex_flags) @Point2/x.cpp -output @Point2/x +@Point2/y.$(MEXENDING): @Point2/y.cpp + $(MEX) $(mex_flags) @Point2/y.cpp -output @Point2/y +@Point2/dim.$(MEXENDING): @Point2/dim.cpp + $(MEX) $(mex_flags) @Point2/dim.cpp -output @Point2/dim +@Point2/vectorConfusion.$(MEXENDING): @Point2/vectorConfusion.cpp + $(MEX) $(mex_flags) @Point2/vectorConfusion.cpp -output @Point2/vectorConfusion + +Point2: new_Point2_.$(MEXENDING) new_Point2_dd.$(MEXENDING) @Point2/x.$(MEXENDING) @Point2/y.$(MEXENDING) @Point2/dim.$(MEXENDING) @Point2/vectorConfusion.$(MEXENDING) + +# Point3 +new_Point3_ddd.$(MEXENDING): new_Point3_ddd.cpp + $(MEX) $(mex_flags) new_Point3_ddd.cpp -output new_Point3_ddd +Point3_staticFunction.$(MEXENDING): Point3_staticFunction.cpp + $(MEX) $(mex_flags) Point3_staticFunction.cpp -output Point3_staticFunction +Point3_StaticFunctionRet.$(MEXENDING): Point3_StaticFunctionRet.cpp + $(MEX) $(mex_flags) Point3_StaticFunctionRet.cpp -output Point3_StaticFunctionRet +@Point3/norm.$(MEXENDING): @Point3/norm.cpp + $(MEX) $(mex_flags) @Point3/norm.cpp -output @Point3/norm + +Point3: new_Point3_ddd.$(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 +@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 + $(MEX) $(mex_flags) @Test/return_bool.cpp -output @Test/return_bool +@Test/return_size_t.$(MEXENDING): @Test/return_size_t.cpp + $(MEX) $(mex_flags) @Test/return_size_t.cpp -output @Test/return_size_t +@Test/return_int.$(MEXENDING): @Test/return_int.cpp + $(MEX) $(mex_flags) @Test/return_int.cpp -output @Test/return_int +@Test/return_double.$(MEXENDING): @Test/return_double.cpp + $(MEX) $(mex_flags) @Test/return_double.cpp -output @Test/return_double +@Test/return_string.$(MEXENDING): @Test/return_string.cpp + $(MEX) $(mex_flags) @Test/return_string.cpp -output @Test/return_string +@Test/return_vector1.$(MEXENDING): @Test/return_vector1.cpp + $(MEX) $(mex_flags) @Test/return_vector1.cpp -output @Test/return_vector1 +@Test/return_matrix1.$(MEXENDING): @Test/return_matrix1.cpp + $(MEX) $(mex_flags) @Test/return_matrix1.cpp -output @Test/return_matrix1 +@Test/return_vector2.$(MEXENDING): @Test/return_vector2.cpp + $(MEX) $(mex_flags) @Test/return_vector2.cpp -output @Test/return_vector2 +@Test/return_matrix2.$(MEXENDING): @Test/return_matrix2.cpp + $(MEX) $(mex_flags) @Test/return_matrix2.cpp -output @Test/return_matrix2 +@Test/arg_EigenConstRef.$(MEXENDING): @Test/arg_EigenConstRef.cpp + $(MEX) $(mex_flags) @Test/arg_EigenConstRef.cpp -output @Test/arg_EigenConstRef +@Test/return_field.$(MEXENDING): @Test/return_field.cpp + $(MEX) $(mex_flags) @Test/return_field.cpp -output @Test/return_field +@Test/return_TestPtr.$(MEXENDING): @Test/return_TestPtr.cpp + $(MEX) $(mex_flags) @Test/return_TestPtr.cpp -output @Test/return_TestPtr +@Test/return_Test.$(MEXENDING): @Test/return_Test.cpp + $(MEX) $(mex_flags) @Test/return_Test.cpp -output @Test/return_Test +@Test/return_Point2Ptr.$(MEXENDING): @Test/return_Point2Ptr.cpp + $(MEX) $(mex_flags) @Test/return_Point2Ptr.cpp -output @Test/return_Point2Ptr +@Test/create_ptrs.$(MEXENDING): @Test/create_ptrs.cpp + $(MEX) $(mex_flags) @Test/create_ptrs.cpp -output @Test/create_ptrs +@Test/create_MixedPtrs.$(MEXENDING): @Test/create_MixedPtrs.cpp + $(MEX) $(mex_flags) @Test/create_MixedPtrs.cpp -output @Test/create_MixedPtrs +@Test/return_ptrs.$(MEXENDING): @Test/return_ptrs.cpp + $(MEX) $(mex_flags) @Test/return_ptrs.cpp -output @Test/return_ptrs +@Test/print.$(MEXENDING): @Test/print.cpp + $(MEX) $(mex_flags) @Test/print.cpp -output @Test/print + +Test: new_Test_.$(MEXENDING) new_Test_dM.$(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) + + + +clean: + rm -rf *.$(MEXENDING) + rm -rf @Point2/*.$(MEXENDING) + rm -rf @Point3/*.$(MEXENDING) + rm -rf @Test/*.$(MEXENDING) + + diff --git a/wrap/tests/expected/Point3_StaticFunctionRet.cpp b/wrap/tests/expected/Point3_StaticFunctionRet.cpp new file mode 100644 index 000000000..657f90bb1 --- /dev/null +++ b/wrap/tests/expected/Point3_StaticFunctionRet.cpp @@ -0,0 +1,11 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("Point3_StaticFunctionRet",nargout,nargin,1); + double z = unwrap< double >(in[0]); + Point3 result = Point3::StaticFunctionRet(z); + out[0] = wrap_shared_ptr(make_shared< Point3 >(result),"Point3"); +} diff --git a/wrap/tests/expected/Point3_StaticFunctionRet.m b/wrap/tests/expected/Point3_StaticFunctionRet.m new file mode 100644 index 000000000..f2ba314f1 --- /dev/null +++ b/wrap/tests/expected/Point3_StaticFunctionRet.m @@ -0,0 +1,4 @@ +function result = Point3_StaticFunctionRet() +% usage: x = Point3_StaticFunctionRet() + error('need to compile Point3_StaticFunctionRet.cpp'); +end diff --git a/wrap/tests/expected/Point3_staticFunction.cpp b/wrap/tests/expected/Point3_staticFunction.cpp new file mode 100644 index 000000000..e0519e20e --- /dev/null +++ b/wrap/tests/expected/Point3_staticFunction.cpp @@ -0,0 +1,10 @@ +// automatically generated by wrap on 2011-Dec-09 +#include +#include +using namespace geometry; +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("Point3_staticFunction",nargout,nargin,0); + double result = Point3::staticFunction(); + out[0] = wrap< double >(result); +} diff --git a/wrap/tests/expected/Point3_staticFunction.m b/wrap/tests/expected/Point3_staticFunction.m new file mode 100644 index 000000000..441bb5bc6 --- /dev/null +++ b/wrap/tests/expected/Point3_staticFunction.m @@ -0,0 +1,4 @@ +function result = Point3_staticFunction() +% usage: x = Point3_staticFunction() + error('need to compile Point3_staticFunction.cpp'); +end diff --git a/wrap/tests/expected/make_geometry.m b/wrap/tests/expected/make_geometry.m index cad4eab1d..39123f65d 100644 --- a/wrap/tests/expected/make_geometry.m +++ b/wrap/tests/expected/make_geometry.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Nov-04 +% automatically generated by wrap on 2011-Dec-06 echo on toolboxpath = mfilename('fullpath'); @@ -16,10 +16,13 @@ cd @Point2 mex -O5 x.cpp mex -O5 y.cpp mex -O5 dim.cpp +mex -O5 vectorConfusion.cpp %% Point3 cd(toolboxpath) mex -O5 new_Point3_ddd.cpp +mex -O5 Point3_staticFunction.cpp +mex -O5 Point3_StaticFunctionRet.cpp cd @Point3 mex -O5 norm.cpp @@ -27,8 +30,10 @@ mex -O5 norm.cpp %% Test cd(toolboxpath) mex -O5 new_Test_.cpp +mex -O5 new_Test_dM.cpp cd @Test +mex -O5 return_pair.cpp mex -O5 return_bool.cpp mex -O5 return_size_t.cpp mex -O5 return_int.cpp @@ -38,10 +43,13 @@ mex -O5 return_vector1.cpp mex -O5 return_matrix1.cpp mex -O5 return_vector2.cpp mex -O5 return_matrix2.cpp -mex -O5 return_pair.cpp +mex -O5 arg_EigenConstRef.cpp mex -O5 return_field.cpp mex -O5 return_TestPtr.cpp +mex -O5 return_Test.cpp +mex -O5 return_Point2Ptr.cpp mex -O5 create_ptrs.cpp +mex -O5 create_MixedPtrs.cpp mex -O5 return_ptrs.cpp mex -O5 print.cpp diff --git a/wrap/tests/expected/new_Point2_.cpp b/wrap/tests/expected/new_Point2_.cpp index 4745ae6a1..5cfcad238 100644 --- a/wrap/tests/expected/new_Point2_.cpp +++ b/wrap/tests/expected/new_Point2_.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Point2_",nargout,nargin,0); diff --git a/wrap/tests/expected/new_Point2_.m b/wrap/tests/expected/new_Point2_.m index 5b3756ef7..dc02b426b 100644 --- a/wrap/tests/expected/new_Point2_.m +++ b/wrap/tests/expected/new_Point2_.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Oct-31 +% automatically generated by wrap on 2011-Dec-06 function result = new_Point2_(obj) 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 index 30db28cd9..6ab5721a1 100644 --- a/wrap/tests/expected/new_Point2_dd.cpp +++ b/wrap/tests/expected/new_Point2_dd.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Point2_dd",nargout,nargin,2); diff --git a/wrap/tests/expected/new_Point2_dd.m b/wrap/tests/expected/new_Point2_dd.m index f83962584..7d4bf7b86 100644 --- a/wrap/tests/expected/new_Point2_dd.m +++ b/wrap/tests/expected/new_Point2_dd.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Oct-31 +% automatically generated by wrap on 2011-Dec-06 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_ddd.cpp b/wrap/tests/expected/new_Point3_ddd.cpp index c83518386..285ed9ca6 100644 --- a/wrap/tests/expected/new_Point3_ddd.cpp +++ b/wrap/tests/expected/new_Point3_ddd.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include #include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Point3_ddd",nargout,nargin,3); diff --git a/wrap/tests/expected/new_Point3_ddd.m b/wrap/tests/expected/new_Point3_ddd.m index 3b15dcfdb..b3004b4bb 100644 --- a/wrap/tests/expected/new_Point3_ddd.m +++ b/wrap/tests/expected/new_Point3_ddd.m @@ -1,4 +1,4 @@ -% automatically generated by wrap on 2011-Oct-31 +% automatically generated by wrap on 2011-Dec-06 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 9afe37223..9c9aaab88 100644 --- a/wrap/tests/expected/new_Test_.cpp +++ b/wrap/tests/expected/new_Test_.cpp @@ -1,6 +1,7 @@ -// automatically generated by wrap on 2011-Oct-31 +// automatically generated by wrap on 2011-Dec-09 #include -#include +#include +using namespace geometry; void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("new_Test_",nargout,nargin,0); diff --git a/wrap/tests/expected/new_Test_.m b/wrap/tests/expected/new_Test_.m index 03f3ca25f..9064b5f23 100644 --- a/wrap/tests/expected/new_Test_.m +++ b/wrap/tests/expected/new_Test_.m @@ -1,4 +1,4 @@ +% automatically generated by wrap on 2011-Dec-06 function result = new_Test_(obj) -% automatically generated by wrap on 2011-Oct-31 error('need to compile new_Test_.cpp'); end diff --git a/wrap/tests/expected/new_Test_b.cpp b/wrap/tests/expected/new_Test_b.cpp new file mode 100644 index 000000000..921c692af --- /dev/null +++ b/wrap/tests/expected/new_Test_b.cpp @@ -0,0 +1,10 @@ +// automatically generated by wrap on 2011-Dec-01 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("new_Test_b",nargout,nargin,1); + bool value = unwrap< bool >(in[0]); + Test* self = new Test(value); + out[0] = wrap_constructed(self,"Test"); +} diff --git a/wrap/tests/expected/new_Test_b.m b/wrap/tests/expected/new_Test_b.m new file mode 100644 index 000000000..a07945d9f --- /dev/null +++ b/wrap/tests/expected/new_Test_b.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-01 +function result = new_Test_b(obj,value) + error('need to compile new_Test_b.cpp'); +end diff --git a/wrap/tests/expected/new_Test_dM.cpp b/wrap/tests/expected/new_Test_dM.cpp new file mode 100644 index 000000000..4179921da --- /dev/null +++ b/wrap/tests/expected/new_Test_dM.cpp @@ -0,0 +1,12 @@ +// automatically generated by wrap on 2011-Dec-09 +#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_namespaces/@ClassD/ClassD.m b/wrap/tests/expected_namespaces/@ClassD/ClassD.m new file mode 100644 index 000000000..d6b5b452a --- /dev/null +++ b/wrap/tests/expected_namespaces/@ClassD/ClassD.m @@ -0,0 +1,13 @@ +classdef ClassD + properties + self = 0 + 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 + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m b/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m new file mode 100644 index 000000000..f5de15c80 --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m @@ -0,0 +1,13 @@ +classdef ns1ClassA + properties + self = 0 + 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 + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m b/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m new file mode 100644 index 000000000..189dab25b --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m @@ -0,0 +1,13 @@ +classdef ns1ClassB + properties + self = 0 + 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 + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp new file mode 100644 index 000000000..d051c74ec --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp @@ -0,0 +1,10 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("memberFunction",nargout,nargin-1,0); + shared_ptr self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); + double result = self->memberFunction(); + out[0] = wrap< double >(result); +} diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.m b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.m new file mode 100644 index 000000000..8e8d24bd7 --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.m @@ -0,0 +1,4 @@ +function result = memberFunction(obj) +% usage: obj.memberFunction() + error('need to compile memberFunction.cpp'); +end diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m b/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m new file mode 100644 index 000000000..f0521377e --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m @@ -0,0 +1,13 @@ +classdef ns2ClassA + properties + self = 0 + 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 + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m b/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m new file mode 100644 index 000000000..78fc02073 --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m @@ -0,0 +1,13 @@ +classdef ns2ClassC + properties + self = 0 + 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 + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m b/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m new file mode 100644 index 000000000..35891c5a7 --- /dev/null +++ b/wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m @@ -0,0 +1,13 @@ +classdef ns2ns3ClassB + properties + self = 0 + 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 + end + function display(obj), obj.print(''); end + function disp(obj), obj.display; end + end +end diff --git a/wrap/tests/expected_namespaces/Makefile b/wrap/tests/expected_namespaces/Makefile new file mode 100644 index 000000000..4fe9b0655 --- /dev/null +++ b/wrap/tests/expected_namespaces/Makefile @@ -0,0 +1,64 @@ +# automatically generated by wrap on 2011-Dec-08 + +MEX = mex +MEXENDING = mexa64 +mex_flags = -O5 + +all: ns1ClassA ns1ClassB ns2ClassA ns2ns3ClassB ns2ClassC ClassD + +# ns1ClassA +new_ns1ClassA_.$(MEXENDING): new_ns1ClassA_.cpp + $(MEX) $(mex_flags) new_ns1ClassA_.cpp -output new_ns1ClassA_ + +ns1ClassA: new_ns1ClassA_.$(MEXENDING) + +# ns1ClassB +new_ns1ClassB_.$(MEXENDING): new_ns1ClassB_.cpp + $(MEX) $(mex_flags) new_ns1ClassB_.cpp -output new_ns1ClassB_ + +ns1ClassB: new_ns1ClassB_.$(MEXENDING) + +# ns2ClassA +new_ns2ClassA_.$(MEXENDING): new_ns2ClassA_.cpp + $(MEX) $(mex_flags) new_ns2ClassA_.cpp -output new_ns2ClassA_ +ns2ClassA_afunction.$(MEXENDING): ns2ClassA_afunction.cpp + $(MEX) $(mex_flags) ns2ClassA_afunction.cpp -output ns2ClassA_afunction +@ns2ClassA/memberFunction.$(MEXENDING): @ns2ClassA/memberFunction.cpp + $(MEX) $(mex_flags) @ns2ClassA/memberFunction.cpp -output @ns2ClassA/memberFunction +@ns2ClassA/nsArg.$(MEXENDING): @ns2ClassA/nsArg.cpp + $(MEX) $(mex_flags) @ns2ClassA/nsArg.cpp -output @ns2ClassA/nsArg +@ns2ClassA/nsReturn.$(MEXENDING): @ns2ClassA/nsReturn.cpp + $(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 + $(MEX) $(mex_flags) new_ns2ns3ClassB_.cpp -output new_ns2ns3ClassB_ + +ns2ns3ClassB: new_ns2ns3ClassB_.$(MEXENDING) + +# ns2ClassC +new_ns2ClassC_.$(MEXENDING): new_ns2ClassC_.cpp + $(MEX) $(mex_flags) new_ns2ClassC_.cpp -output new_ns2ClassC_ + +ns2ClassC: new_ns2ClassC_.$(MEXENDING) + +# ClassD +new_ClassD_.$(MEXENDING): new_ClassD_.cpp + $(MEX) $(mex_flags) new_ClassD_.cpp -output new_ClassD_ + +ClassD: new_ClassD_.$(MEXENDING) + + + +clean: + rm -rf *.$(MEXENDING) + rm -rf @ns1ClassA/*.$(MEXENDING) + rm -rf @ns1ClassB/*.$(MEXENDING) + rm -rf @ns2ClassA/*.$(MEXENDING) + rm -rf @ns2ns3ClassB/*.$(MEXENDING) + rm -rf @ns2ClassC/*.$(MEXENDING) + rm -rf @ClassD/*.$(MEXENDING) + + diff --git a/wrap/tests/expected_namespaces/make_testNamespaces.m b/wrap/tests/expected_namespaces/make_testNamespaces.m new file mode 100644 index 000000000..59d850c42 --- /dev/null +++ b/wrap/tests/expected_namespaces/make_testNamespaces.m @@ -0,0 +1,52 @@ +% automatically generated by wrap on 2011-Dec-08 +echo on + +toolboxpath = mfilename('fullpath'); +delims = find(toolboxpath == '/'); +toolboxpath = toolboxpath(1:(delims(end)-1)); +clear delims +addpath(toolboxpath); + +%% ns1ClassA +cd(toolboxpath) +mex -O5 new_ns1ClassA_.cpp + +cd @ns1ClassA + +%% ns1ClassB +cd(toolboxpath) +mex -O5 new_ns1ClassB_.cpp + +cd @ns1ClassB + +%% ns2ClassA +cd(toolboxpath) +mex -O5 new_ns2ClassA_.cpp +mex -O5 ns2ClassA_afunction.cpp + +cd @ns2ClassA +mex -O5 memberFunction.cpp +mex -O5 nsArg.cpp +mex -O5 nsReturn.cpp + +%% ns2ns3ClassB +cd(toolboxpath) +mex -O5 new_ns2ns3ClassB_.cpp + +cd @ns2ns3ClassB + +%% ns2ClassC +cd(toolboxpath) +mex -O5 new_ns2ClassC_.cpp + +cd @ns2ClassC + +%% ClassD +cd(toolboxpath) +mex -O5 new_ClassD_.cpp + +cd @ClassD + +cd(toolboxpath) + +echo off diff --git a/wrap/tests/expected_namespaces/new_ClassD_.cpp b/wrap/tests/expected_namespaces/new_ClassD_.cpp new file mode 100644 index 000000000..ec7212786 --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ClassD_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +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"); +} diff --git a/wrap/tests/expected_namespaces/new_ClassD_.m b/wrap/tests/expected_namespaces/new_ClassD_.m new file mode 100644 index 000000000..c5f53f130 --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ClassD_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ClassD_(obj) + error('need to compile new_ClassD_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp b/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp new file mode 100644 index 000000000..2db8ef767 --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns1ClassA_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +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"); +} diff --git a/wrap/tests/expected_namespaces/new_ns1ClassA_.m b/wrap/tests/expected_namespaces/new_ns1ClassA_.m new file mode 100644 index 000000000..89cd8b0a2 --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns1ClassA_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns1ClassA_(obj) + error('need to compile new_ns1ClassA_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp b/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp new file mode 100644 index 000000000..b4ac7038a --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns1ClassB_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +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"); +} diff --git a/wrap/tests/expected_namespaces/new_ns1ClassB_.m b/wrap/tests/expected_namespaces/new_ns1ClassB_.m new file mode 100644 index 000000000..5430f85aa --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns1ClassB_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns1ClassB_(obj) + error('need to compile new_ns1ClassB_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp b/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp new file mode 100644 index 000000000..cc4ec309b --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ClassA_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +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"); +} diff --git a/wrap/tests/expected_namespaces/new_ns2ClassA_.m b/wrap/tests/expected_namespaces/new_ns2ClassA_.m new file mode 100644 index 000000000..bb8b2a24a --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ClassA_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns2ClassA_(obj) + error('need to compile new_ns2ClassA_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp b/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp new file mode 100644 index 000000000..b43a7cd6b --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ClassC_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +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"); +} diff --git a/wrap/tests/expected_namespaces/new_ns2ClassC_.m b/wrap/tests/expected_namespaces/new_ns2ClassC_.m new file mode 100644 index 000000000..91e643c4b --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ClassC_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns2ClassC_(obj) + error('need to compile new_ns2ClassC_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp new file mode 100644 index 000000000..3916ed3ff --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +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"); +} diff --git a/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.m b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.m new file mode 100644 index 000000000..54b38a16c --- /dev/null +++ b/wrap/tests/expected_namespaces/new_ns2ns3ClassB_.m @@ -0,0 +1,4 @@ +% automatically generated by wrap on 2011-Dec-08 +function result = new_ns2ns3ClassB_(obj) + error('need to compile new_ns2ns3ClassB_.cpp'); +end diff --git a/wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp b/wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp new file mode 100644 index 000000000..dff68090a --- /dev/null +++ b/wrap/tests/expected_namespaces/ns2ClassA_afunction.cpp @@ -0,0 +1,9 @@ +// automatically generated by wrap on 2011-Dec-08 +#include +#include +void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("ns2ClassA_afunction",nargout,nargin,0); + double result = ns2::ClassA::afunction(); + out[0] = wrap< double >(result); +} diff --git a/wrap/tests/expected_namespaces/ns2ClassA_afunction.m b/wrap/tests/expected_namespaces/ns2ClassA_afunction.m new file mode 100644 index 000000000..0482d1548 --- /dev/null +++ b/wrap/tests/expected_namespaces/ns2ClassA_afunction.m @@ -0,0 +1,4 @@ +function result = ns2ClassA_afunction() +% usage: x = ns2ClassA_afunction() + error('need to compile ns2ClassA_afunction.cpp'); +end diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h new file mode 100644 index 000000000..d77d6ee77 --- /dev/null +++ b/wrap/tests/geometry.h @@ -0,0 +1,80 @@ + // comments! + +// set the default namespace +// location of namespace isn't significant +using namespace geometry; + +class Point2 { + Point2(); + Point2(double x, double y); + double x() const; + double y() const; + int dim() const; + VectorNotEigen vectorConfusion(); +}; + +class Point3 { + Point3(double x, double y, double z); + double norm() const; + + // static functions - use static keyword and uppercase + static double staticFunction(); + static Point3 StaticFunctionRet(double z); +}; + +// another comment + +// another comment + +/** + * A multi-line comment! + */ + +class Test { +#include + + /* a comment! */ + // another comment + Test(); + + pair return_pair (Vector v, Matrix A) const; // intentionally the first method + + bool return_bool (bool value) const; // comment after a line! + size_t return_size_t (size_t value) const; + int return_int (int value) const; + double return_double (double value) const; + + Test(double a, Matrix b); // a constructor in the middle of a class + + // comments in the middle! + + // (more) comments in the middle! + + string return_string (string value) const; + Vector return_vector1(Vector value) const; + Matrix return_matrix1(Matrix value) const; + Vector return_vector2(Vector value) const; + Matrix return_matrix2(Matrix value) const; + void arg_EigenConstRef(const Matrix& value) const; + + bool return_field(const Test& t) const; + + Test* return_TestPtr(Test* value) const; + Test return_Test(Test* value) const; + + Point2* return_Point2Ptr(bool value) const; + + pair create_ptrs () const; + pair create_MixedPtrs () const; + pair return_ptrs (Test* p1, Test* p2) const; + + void print() const; + + // comments at the end! + + // even more comments at the end! +}; + +// comments at the end! + +// even more comments at the end! diff --git a/wrap/tests/testNamespaces.h b/wrap/tests/testNamespaces.h new file mode 100644 index 000000000..f27a518c6 --- /dev/null +++ b/wrap/tests/testNamespaces.h @@ -0,0 +1,45 @@ +/** + * This is a wrap header to verify permutations on namespaces + */ + +namespace ns1 { + +class ClassA { + ClassA(); +}; + +class ClassB { + ClassB(); +}; + +}///\namespace ns1 + +namespace ns2 { + +class ClassA { + ClassA(); + static double afunction(); + double memberFunction(); + int nsArg(const ns1::ClassB& arg); + ns2::ns3::ClassB nsReturn(double q); +}; + +namespace ns3 { + +class ClassB { + ClassB(); +}; + +}///\namespace ns3 + +class ClassC { + ClassC(); +}; + +}///\namespace ns2 + +class ClassD { + ClassD(); +}; + + diff --git a/wrap/tests/testSpirit.cpp b/wrap/tests/testSpirit.cpp index 3f68952f3..d4a6d151e 100644 --- a/wrap/tests/testSpirit.cpp +++ b/wrap/tests/testSpirit.cpp @@ -18,6 +18,7 @@ #include #include #include +#include using namespace std; using namespace BOOST_SPIRIT_CLASSIC_NS; @@ -36,31 +37,31 @@ Rule basisType_p = (str_p("string") | "bool" | "size_t" | "int" | "double" | "Ve /* ************************************************************************* */ TEST( spirit, real ) { // check if we can parse 8.99 as a real - CHECK(parse("8.99", real_p, space_p).full); + EXPECT(parse("8.99", real_p, space_p).full); // make sure parsing fails on this one - CHECK(!parse("zztop", real_p, space_p).full); + EXPECT(!parse("zztop", real_p, space_p).full); } /* ************************************************************************* */ TEST( spirit, string ) { // check if we can parse a string - CHECK(parse("double", str_p("double"), space_p).full); + EXPECT(parse("double", str_p("double"), space_p).full); } /* ************************************************************************* */ TEST( spirit, sequence ) { // check that we skip white space - CHECK(parse("int int", str_p("int") >> *str_p("int"), space_p).full); - CHECK(parse("int --- - -- -", str_p("int") >> *ch_p('-'), space_p).full); - CHECK(parse("const \t string", str_p("const") >> str_p("string"), space_p).full); + EXPECT(parse("int int", str_p("int") >> *str_p("int"), space_p).full); + EXPECT(parse("int --- - -- -", str_p("int") >> *ch_p('-'), space_p).full); + EXPECT(parse("const \t string", str_p("const") >> str_p("string"), space_p).full); - // not that (see spirit FAQ) the vanilla rule<> does not deal with whitespace + // note that (see spirit FAQ) the vanilla rule<> does not deal with whitespace rule<>vanilla_p = str_p("const") >> str_p("string"); - CHECK(!parse("const \t string", vanilla_p, space_p).full); + EXPECT(!parse("const \t string", vanilla_p, space_p).full); // to fix it, we need to use rulephrase_level_p = str_p("const") >> str_p("string"); - CHECK(parse("const \t string", phrase_level_p, space_p).full); + EXPECT(parse("const \t string", phrase_level_p, space_p).full); } /* ************************************************************************* */ @@ -81,24 +82,58 @@ Rule constMethod_p = basisType_p >> methodName_p >> '(' >> ')' >> "const" >> ';' /* ************************************************************************* */ TEST( spirit, basisType_p ) { - CHECK(!parse("Point3", basisType_p, space_p).full); - CHECK(parse("string", basisType_p, space_p).full); + EXPECT(!parse("Point3", basisType_p, space_p).full); + EXPECT(parse("string", basisType_p, space_p).full); } /* ************************************************************************* */ TEST( spirit, className_p ) { - CHECK(parse("Point3", className_p, space_p).full); + EXPECT(parse("Point3", className_p, space_p).full); } /* ************************************************************************* */ TEST( spirit, classRef_p ) { - CHECK(parse("Point3 &", classRef_p, space_p).full); - CHECK(parse("Point3&", classRef_p, space_p).full); + EXPECT(parse("Point3 &", classRef_p, space_p).full); + EXPECT(parse("Point3&", classRef_p, space_p).full); } /* ************************************************************************* */ TEST( spirit, constMethod_p ) { - CHECK(parse("double norm() const;", constMethod_p, space_p).full); + EXPECT(parse("double norm() const;", constMethod_p, space_p).full); +} + +/* ************************************************************************* */ +TEST( spirit, return_value_p ) { + bool isEigen = true; + string actual_return_type; + string actual_function_name; + + Rule basisType_p = + (str_p("string") | "bool" | "size_t" | "int" | "double"); + + Rule eigenType_p = + (str_p("Vector") | "Matrix"); + + Rule className_p = lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - basisType_p; + + Rule funcName_p = lexeme_d[lower_p >> *(alnum_p | '_')]; + + Rule returnType_p = + (basisType_p[assign_a(actual_return_type)][assign_a(isEigen, true)]) | + (className_p[assign_a(actual_return_type)][assign_a(isEigen,false)]) | + (eigenType_p[assign_a(actual_return_type)][assign_a(isEigen, true)]); + + Rule testFunc_p = returnType_p >> funcName_p[assign_a(actual_function_name)] >> str_p("();"); + + EXPECT(parse("VectorNotEigen doesNotReturnAnEigenVector();", testFunc_p, space_p).full); + EXPECT(!isEigen); + EXPECT(actual_return_type == "VectorNotEigen"); + EXPECT(actual_function_name == "doesNotReturnAnEigenVector"); + + EXPECT(parse("Vector actuallyAVector();", testFunc_p, space_p).full); + EXPECT(isEigen); + EXPECT(actual_return_type == "Vector"); + EXPECT(actual_function_name == "actuallyAVector"); } /* ************************************************************************* */ diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 0650538a5..d4277533a 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -15,6 +15,7 @@ * @author Frank Dellaert **/ +#include #include #include #include @@ -24,78 +25,186 @@ #include using namespace std; -static bool verbose = false; +using namespace wrap; +static bool enable_verbose = false; #ifdef TOPSRCDIR static string topdir = TOPSRCDIR; #else -static string topdir = "notarealdirectory"; // If TOPSRCDIR is not defined, we error +static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not defined, we error #endif /* ************************************************************************* */ TEST( wrap, ArgumentList ) { - ArgumentList args; - Argument arg; arg.type = "double"; arg.name = "x"; - args.push_back(arg); - args.push_back(arg); - args.push_back(arg); - CHECK(args.signature()=="ddd"); - EXPECT(args.types()=="double,double,double"); - EXPECT(args.names()=="x,x,x"); + ArgumentList args; + Argument arg; arg.type = "double"; arg.name = "x"; + args.push_back(arg); + args.push_back(arg); + args.push_back(arg); + CHECK(args.signature()=="ddd"); + EXPECT(args.types()=="double,double,double"); + EXPECT(args.names()=="x,x,x"); } /* ************************************************************************* */ TEST( wrap, check_exception ) { - THROWS_EXCEPTION(Module("/notarealpath", "geometry",verbose)); - CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",verbose), CantOpenFile); + THROWS_EXCEPTION(Module("/notarealpath", "geometry",enable_verbose)); + CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",enable_verbose), CantOpenFile); + + // clean out previous generated code + string cleanCmd = "rm -rf actual"; + system(cleanCmd.c_str()); + + string path = topdir + "/wrap/tests"; + Module module(path.c_str(), "testWrap1",enable_verbose); + CHECK_EXCEPTION(module.matlab_code("actual", "mexa64", "-O5"), DependencyMissing); } /* ************************************************************************* */ TEST( wrap, parse ) { - string path = topdir + "/wrap"; + string header_path = topdir + "/wrap/tests"; + Module module(header_path.c_str(), "geometry",enable_verbose); + EXPECT_LONGS_EQUAL(3, module.classes.size()); - Module module(path.c_str(), "geometry",verbose); - EXPECT(module.classes.size()==3); + // check using declarations + EXPECT_LONGS_EQUAL(1, module.using_namespaces.size()); + EXPECT(assert_equal("geometry", module.using_namespaces.front())); + + // check first class, Point2 + { + Class cls = module.classes.at(0); + EXPECT(assert_equal("Point2", cls.name)); + EXPECT_LONGS_EQUAL(2, cls.constructors.size()); + EXPECT_LONGS_EQUAL(4, cls.methods.size()); + EXPECT_LONGS_EQUAL(0, cls.static_methods.size()); + EXPECT_LONGS_EQUAL(0, cls.namespaces.size()); + } // check second class, Point3 - Class cls = *(++module.classes.begin()); - EXPECT(cls.name=="Point3"); - EXPECT(cls.constructors.size()==1); - EXPECT(cls.methods.size()==1); + { + Class cls = module.classes.at(1); + EXPECT(assert_equal("Point3", cls.name)); + EXPECT_LONGS_EQUAL(1, cls.constructors.size()); + EXPECT_LONGS_EQUAL(1, cls.methods.size()); + EXPECT_LONGS_EQUAL(2, cls.static_methods.size()); + EXPECT_LONGS_EQUAL(0, cls.namespaces.size()); - // first constructor takes 3 doubles - Constructor c1 = cls.constructors.front(); - EXPECT(c1.args.size()==3); + // first constructor takes 3 doubles + Constructor c1 = cls.constructors.front(); + EXPECT_LONGS_EQUAL(3, c1.args.size()); - // check first double argument - Argument a1 = c1.args.front(); - EXPECT(!a1.is_const); - EXPECT(a1.type=="double"); - EXPECT(!a1.is_ref); - EXPECT(a1.name=="x"); + // check first double argument + Argument a1 = c1.args.front(); + EXPECT(!a1.is_const); + EXPECT(assert_equal("double", a1.type)); + EXPECT(!a1.is_ref); + EXPECT(assert_equal("x", a1.name)); - // check method - Method m1 = cls.methods.front(); - EXPECT(m1.returns_=="double"); - EXPECT(m1.name_=="norm"); - EXPECT(m1.args_.size()==0); - EXPECT(m1.is_const_); + // check method + Method m1 = cls.methods.front(); + EXPECT(assert_equal("double", m1.returnVal.type1)); + EXPECT(assert_equal("norm", m1.name)); + EXPECT_LONGS_EQUAL(0, m1.args.size()); + EXPECT(m1.is_const_); + } + + // Test class is the third one + { + LONGS_EQUAL(3, module.classes.size()); + Class testCls = module.classes.at(2); + EXPECT_LONGS_EQUAL( 2, testCls.constructors.size()); + EXPECT_LONGS_EQUAL(19, testCls.methods.size()); + EXPECT_LONGS_EQUAL( 0, testCls.static_methods.size()); + EXPECT_LONGS_EQUAL( 0, testCls.namespaces.size()); + EXPECT_LONGS_EQUAL( 1, testCls.includes.size()); + EXPECT(assert_equal("folder/path/to/Test.h", testCls.includes.front())); + + // function to parse: pair return_pair (Vector v, Matrix A) const; + Method m2 = testCls.methods.front(); + EXPECT(m2.returnVal.isPair); + EXPECT(m2.returnVal.category1 == ReturnValue::EIGEN); + EXPECT(m2.returnVal.category2 == ReturnValue::EIGEN); + } +} + +/* ************************************************************************* */ +TEST( wrap, parse_namespaces ) { + string header_path = topdir + "/wrap/tests"; + Module module(header_path.c_str(), "testNamespaces",enable_verbose); + EXPECT_LONGS_EQUAL(6, module.classes.size()); + + Class cls1 = module.classes.at(0); + EXPECT(assert_equal("ClassA", cls1.name)); + EXPECT_LONGS_EQUAL(1, cls1.namespaces.size()); + EXPECT(assert_equal("ns1", cls1.namespaces.front())); + + Class cls2 = module.classes.at(1); + EXPECT(assert_equal("ClassB", cls2.name)); + EXPECT_LONGS_EQUAL(1, cls2.namespaces.size()); + EXPECT(assert_equal("ns1", cls2.namespaces.front())); + + Class cls3 = module.classes.at(2); + EXPECT(assert_equal("ClassA", cls3.name)); + EXPECT_LONGS_EQUAL(1, cls3.namespaces.size()); + EXPECT(assert_equal("ns2", cls3.namespaces.front())); + + Class cls4 = module.classes.at(3); + EXPECT(assert_equal("ClassB", cls4.name)); + EXPECT_LONGS_EQUAL(2, cls4.namespaces.size()); + EXPECT(assert_equal("ns2", cls4.namespaces.front())); + EXPECT(assert_equal("ns3", cls4.namespaces.back())); + + Class cls5 = module.classes.at(4); + EXPECT(assert_equal("ClassC", cls5.name)); + EXPECT_LONGS_EQUAL(1, cls5.namespaces.size()); + EXPECT(assert_equal("ns2", cls5.namespaces.front())); + + Class cls6 = module.classes.at(5); + EXPECT(assert_equal("ClassD", cls6.name)); + EXPECT_LONGS_EQUAL(0, cls6.namespaces.size()); +} + +/* ************************************************************************* */ +TEST( wrap, matlab_code_namespaces ) { + string header_path = topdir + "/wrap/tests"; + Module module(header_path.c_str(), "testNamespaces",enable_verbose); + EXPECT_LONGS_EQUAL(6, module.classes.size()); + string path = topdir + "/wrap"; + + // clean out previous generated code + string cleanCmd = "rm -rf actual_namespaces"; + system(cleanCmd.c_str()); + + // emit MATLAB code + string exp_path = path + "/tests/expected_namespaces/"; + string act_path = "actual_namespaces/"; + module.matlab_code("actual_namespaces", "mexa64", "-O5"); + + EXPECT(files_equal(exp_path + "make_testNamespaces.m", act_path + "make_testNamespaces.m")); + EXPECT(files_equal(exp_path + "Makefile" , act_path + "Makefile" )); } /* ************************************************************************* */ TEST( wrap, matlab_code ) { // Parse into class object + string header_path = topdir + "/wrap/tests"; + Module module(header_path,"geometry",enable_verbose); string path = topdir + "/wrap"; - Module module(path,"geometry",verbose); + + // clean out previous generated code + string cleanCmd = "rm -rf actual"; + system(cleanCmd.c_str()); // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make - module.matlab_code("actual", "", "-O5"); + module.matlab_code("actual", "mexa64", "-O5"); 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" )); @@ -103,13 +212,17 @@ TEST( wrap, matlab_code ) { 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(path + "/tests/expected/make_geometry.m" , "actual/make_geometry.m" )); + EXPECT(files_equal(path + "/tests/expected/make_geometry.m", "actual/make_geometry.m")); + EXPECT(files_equal(path + "/tests/expected/Makefile" , "actual/Makefile" )); } /* ************************************************************************* */ diff --git a/wrap/tests/testWrap1.h b/wrap/tests/testWrap1.h new file mode 100644 index 000000000..0cc16fc71 --- /dev/null +++ b/wrap/tests/testWrap1.h @@ -0,0 +1,23 @@ +//Header file to test dependency checking +// +class Pose3 { + Pose3(); + Pose3(const Rot3& r, const Point3& t);//What is Rot3? Throw here + Pose3(Vector v); + Pose3(Matrix t); + static Pose3 Expmap(Vector v); + static Vector Logmap(const Pose3& p); + static Rot3 testStaticDep(Rot3& r);//What is Rot3? Throw here + void print(string s) const; + bool equals(const Pose3& pose, double tol) const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + Matrix adjointMap() const; + Pose3 compose(const Pose3& p2); + Pose3 between(const Pose3& p2); + Pose3 retract(Vector v); + Point3 translation() const; + Rot3 rotation() const; //What is Rot3? Throw here +}; diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 72833f79a..ebf1ad020 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -15,12 +15,14 @@ **/ #include -#include +#include #include #include "utilities.h" +namespace wrap { + using namespace std; using namespace boost::gregorian; @@ -39,6 +41,16 @@ string file_contents(const string& filename, bool skipheader) { return ss.str(); } +/* ************************************************************************* */ +bool assert_equal(const string& expected, const string& actual) { + if (expected == actual) + return true; + printf("Not equal:\n"); + cout << "expected: [" << expected << "]\n"; + cout << "actual: [" << actual << "]" << endl; + return false; +} + /* ************************************************************************* */ bool files_equal(const string& expected, const string& actual, bool skipheader) { try { @@ -56,13 +68,46 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) cerr << "expection: " << reason << endl; return false; } + catch (CantOpenFile& e) { + cerr << "file opening error: " << e.what() << endl; + return false; + } return true; } /* ************************************************************************* */ -void emit_header_comment(ofstream& ofs, const string& delimiter) { +void generateHeaderComment(ofstream& ofs, const string& delimiter) { date today = day_clock::local_day(); ofs << delimiter << " automatically generated by wrap on " << today << endl; } /* ************************************************************************* */ +string maybe_shared_ptr(bool add, const string& type) { + string str = add? "shared_ptr<" : ""; + str += type; + if (add) str += ">"; + return str; +} + +/* ************************************************************************* */ +void generateUsingNamespace(ofstream& ofs, const vector& using_namespaces) { + if (using_namespaces.empty()) return; + BOOST_FOREACH(const string& s, using_namespaces) + ofs << "using namespace " << s << ";" << endl; +} + +/* ************************************************************************* */ +void generateIncludes(std::ofstream& ofs, const std::string& class_name, + const std::vector& includes) { + ofs << "#include " << endl; + if (includes.empty()) // add a default include + ofs << "#include <" << class_name << ".h>" << endl; + else { + BOOST_FOREACH(const string& s, includes) + ofs << "#include <" << s << ">" << endl; + } +} + +/* ************************************************************************* */ + +} // \namespace wrap diff --git a/wrap/utilities.h b/wrap/utilities.h index d3ab3e197..3837c2b95 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -16,9 +16,13 @@ #pragma once +#include #include +#include #include +namespace wrap { + class CantOpenFile : public std::exception { private: std::string filename_; @@ -38,11 +42,28 @@ class ParseFailed : public std::exception { ~ParseFailed() throw() {} virtual const char* what() const throw() { std::stringstream buf; - buf << "Parse failed at character " << (length_+1); + int len = length_+1; + buf << "Parse failed at character [" << len << "]"; return buf.str().c_str(); } }; +class DependencyMissing : public std::exception { + private: + std::string dependency_; + std::string location_; + public: + DependencyMissing(const std::string& dep, const std::string& loc) { + dependency_ = dep; + location_ = loc; + } + ~DependencyMissing() throw() {} + virtual const char* what() const throw() { + return ("Missing dependency " + dependency_ + " in " + location_).c_str(); + } +}; + + /** * read contents of a file into a std::string */ @@ -54,7 +75,27 @@ std::string file_contents(const std::string& filename, bool skipheader=false); */ bool files_equal(const std::string& expected, const std::string& actual, bool skipheader=true); +/** + * Compare strings for unit tests + */ +bool assert_equal(const std::string& expected, const std::string& actual); /** * emit a header at the top of generated files */ -void emit_header_comment(std::ofstream& ofs, const std::string& delimiter); +void generateHeaderComment(std::ofstream& ofs, const std::string& delimiter); + +// auxiliary function to wrap an argument into a shared_ptr template +std::string maybe_shared_ptr(bool add, const std::string& type); + +/** + * Creates the "using namespace [name];" declarations + */ +void generateUsingNamespace(std::ofstream& ofs, const std::vector& using_namespaces); + +/** + * Creates the #include statements + */ +void generateIncludes(std::ofstream& ofs, const std::string& class_name, + const std::vector& includes); + +} // \namespace wrap diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index d18664cda..e2aaa6aed 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file wrap.ccp + * @file wrap.cpp * @brief wraps functions * @author Frank Dellaert **/ @@ -24,38 +24,39 @@ using namespace std; /** * Top-level function to wrap a module + * @param mexExt is the extension for mex binaries for this os/cpu * @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 mexFlags extra arguments for mex script, i.e., include flags etc... */ -void generate_matlab_toolbox(const string& interfacePath, +void generate_matlab_toolbox(const string& mexExt, + const string& interfacePath, const string& moduleName, const string& toolboxPath, - const string& nameSpace, const string& mexFlags) { // Parse interface file into class object // This recursively creates Class objects, Method objects, etc... - Module module(interfacePath, moduleName, true); + wrap::Module module(interfacePath, moduleName, true); // Then emit MATLAB code - module.matlab_code(toolboxPath,nameSpace,mexFlags); + module.matlab_code(toolboxPath,mexExt,mexFlags); } /** * main parses arguments and calls generate_matlab_toolbox above - * Typyically called from "make all" using appropriate arguments + * Typically called from "make all" using appropriate arguments */ int main(int argc, const char* argv[]) { - if (argc<5 || argc>6) { + if (argc<6 || argc>7) { cerr << "wrap parses an interface file and produces a MATLAB toolbox" << endl; cerr << "usage: wrap interfacePath moduleName toolboxPath" << endl; + cerr << " mexExtension : OS/CPU-dependent extension for MEX binaries" << endl; 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 << " nameSpace : namespace to use, pass empty string if none" << endl; cerr << " [mexFlags] : extra flags for the mex command" << endl; } else From 8d5facb09ef767ad9c1e593401412b0497ffa6ef Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 12 Dec 2011 23:19:31 +0000 Subject: [PATCH 07/17] (in branch) make check passes after refactoring and code changes for including -inl.h files from .h files --- .cproject | 6 +- .project | 2 +- gtsam/base/BTree.h | 2 +- gtsam/base/DSF.h | 2 +- gtsam/inference/BayesNet-inl.h | 1 - gtsam/inference/BayesNet.h | 4 +- gtsam/inference/BayesTree-inl.h | 4 +- gtsam/inference/BayesTree.h | 31 +++++-- gtsam/inference/BayesTreeCliqueBase-inl.h | 93 ++++++++++--------- gtsam/inference/BayesTreeCliqueBase.h | 58 ++++++------ gtsam/inference/EliminationTree-inl.h | 1 - gtsam/inference/EliminationTree.h | 4 +- gtsam/inference/FactorGraph-inl.h | 2 +- gtsam/inference/FactorGraph.h | 1 + gtsam/inference/GenericSequentialSolver-inl.h | 21 ++--- gtsam/inference/GenericSequentialSolver.h | 36 ++++--- gtsam/inference/ISAM-inl.h | 2 +- gtsam/inference/JunctionTree-inl.h | 4 +- gtsam/inference/JunctionTree.h | 2 + gtsam/inference/SymbolicFactorGraph.cpp | 11 +-- gtsam/inference/SymbolicFactorGraph.h | 11 ++- .../inference/SymbolicMultifrontalSolver.cpp | 2 +- gtsam/inference/inference.h | 3 +- gtsam/inference/tests/testClusterTree.cpp | 2 +- gtsam/inference/tests/testJunctionTree.cpp | 2 + gtsam/linear/GaussianConditional.h | 2 +- gtsam/linear/KalmanFilter.h | 2 + .../linear/tests/testGaussianConditional.cpp | 1 + gtsam/linear/tests/testKalmanFilter.cpp | 2 + ...{GaussianISAM2.cpp => GaussianISAM2-inl.h} | 21 +++-- gtsam/nonlinear/GaussianISAM2.h | 11 ++- gtsam/nonlinear/ISAM2-impl-inl.h | 8 +- gtsam/nonlinear/ISAM2-inl.h | 3 +- gtsam/nonlinear/ISAM2.h | 44 ++++++--- gtsam/nonlinear/Makefile.am | 2 +- gtsam/nonlinear/NonlinearFactor.h | 4 +- gtsam/nonlinear/Ordering.h | 1 + gtsam/nonlinear/tests/testOrdering.cpp | 1 + tests/testGaussianISAM2.cpp | 9 +- 39 files changed, 246 insertions(+), 172 deletions(-) rename gtsam/nonlinear/{GaussianISAM2.cpp => GaussianISAM2-inl.h} (84%) diff --git a/.cproject b/.cproject index 35f7946a4..857a2d913 100644 --- a/.cproject +++ b/.cproject @@ -21,7 +21,7 @@ - + @@ -102,7 +102,9 @@ - + + + diff --git a/.project b/.project index d20fd358e..ceef32086 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - -j2 + -j6 org.eclipse.cdt.make.core.buildCommand diff --git a/gtsam/base/BTree.h b/gtsam/base/BTree.h index e0bb4c4a3..2a40465a3 100644 --- a/gtsam/base/BTree.h +++ b/gtsam/base/BTree.h @@ -147,7 +147,7 @@ namespace gtsam { /** add a key-value pair */ BTree add(const KEY& x, const VALUE& d) const { - return add(make_pair(x, d)); + return add(std::make_pair(x, d)); } /** member predicate */ diff --git a/gtsam/base/DSF.h b/gtsam/base/DSF.h index ddc5b4836..9952f2e10 100644 --- a/gtsam/base/DSF.h +++ b/gtsam/base/DSF.h @@ -59,7 +59,7 @@ namespace gtsam { DSF(const std::set& keys) : Tree() { BOOST_FOREACH(const KEY& key, keys) *this = this->add(key, key); } // create a new singleton, does nothing if already exists - Self makeSet(const KEY& key) const { if (mem(key)) return *this; else return this->add(key, key); } + Self makeSet(const KEY& key) const { if (this->mem(key)) return *this; else return this->add(key, key); } // find the label of the set in which {key} lives Label findSet(const KEY& key) const { diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index df9e74717..7ff8fc076 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -18,7 +18,6 @@ #pragma once #include -#include #include #include diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 7b9a85122..672d9a4e8 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -206,4 +206,6 @@ private: } }; // BayesNet -} /// namespace gtsam +} // namespace gtsam + +#include diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index c003102e4..7f77fa898 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include @@ -501,7 +501,7 @@ namespace gtsam { this->removeClique(clique); // remove path above me - this->removePath(typename BayesTreeClique::shared_ptr(clique->parent_.lock()), bn, orphans); + this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans); // add children to list of orphans (splice also removed them from clique->children_) orphans.splice (orphans.begin(), clique->children_); diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 8cb9e3278..9106949b1 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -52,7 +52,7 @@ namespace gtsam { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; typedef boost::shared_ptr sharedConditional; typedef boost::shared_ptr > sharedBayesNet; typedef CONDITIONAL ConditionalType; @@ -178,11 +178,23 @@ namespace gtsam { /** check equality */ bool equals(const BayesTree& other, double tol = 1e-9) const; - /** deep copy from another tree */ void cloneTo(shared_ptr& newTree) const { - root_->cloneToBayesTree(*newTree); + cloneTo(newTree, root()); } + private: + /** deep copy from another tree */ + void cloneTo(shared_ptr& newTree, const sharedClique& root) const { + if(root) { + sharedClique newClique = root->clone(); + newTree->insert(newClique); + BOOST_FOREACH(const sharedClique& childClique, root->children()) { + cloneTo(newTree, childClique); + } + } + } + public: + /** * Find parent clique of a conditional. It will look at all parents and * return the one with the lowest index in the ordering. @@ -315,14 +327,16 @@ namespace gtsam { * extra data along with the clique. */ template - struct BayesTreeClique : public BayesTreeCliqueBase > { + struct BayesTreeClique : public BayesTreeCliqueBase, CONDITIONAL> { public: typedef CONDITIONAL ConditionalType; typedef BayesTreeClique This; - typedef BayesTreeCliqueBase Base; + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; BayesTreeClique() {} - BayesTreeClique(const sharedConditional& conditional) : Base(conditional) {} + BayesTreeClique(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {} + BayesTreeClique(const std::pair& result) : Base(result) {} private: /** Serialization function */ @@ -333,6 +347,7 @@ namespace gtsam { } }; -#include - } /// namespace gtsam + +#include +#include diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index 2956a6cc2..d2d2cec65 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -17,11 +17,13 @@ #pragma once +#include + namespace gtsam { /* ************************************************************************* */ - template - void BayesTreeClique::assertInvariants() const { + template + void BayesTreeCliqueBase::assertInvariants() const { #ifndef NDEBUG // We rely on the keys being sorted // FastVector sortedUniqueKeys(conditional_->begin(), conditional_->end()); @@ -33,52 +35,60 @@ namespace gtsam { } /* ************************************************************************* */ - template - BayesTreeClique::BayesTreeClique(const sharedConditional& conditional) : conditional_(conditional) { + template + BayesTreeCliqueBase::BayesTreeCliqueBase(const sharedConditional& conditional) : + conditional_(conditional) { assertInvariants(); } /* ************************************************************************* */ - template - void BayesTreeClique::print(const string& s) const { + template + BayesTreeCliqueBase::BayesTreeCliqueBase(const std::pair >& result) : + conditional_(result.first) { + assertInvariants(); + } + + /* ************************************************************************* */ + template + void BayesTreeCliqueBase::print(const std::string& s) const { conditional_->print(s); } /* ************************************************************************* */ - template - size_t BayesTreeClique::treeSize() const { + template + size_t BayesTreeCliqueBase::treeSize() const { size_t size = 1; - BOOST_FOREACH(const shared_ptr& child, children_) + BOOST_FOREACH(const derived_ptr& child, children_) size += child->treeSize(); return size; } /* ************************************************************************* */ - template - void BayesTreeClique::printTree(const string& indent) const { + template + void BayesTreeCliqueBase::printTree(const std::string& indent) const { print(indent); - BOOST_FOREACH(const shared_ptr& child, children_) + BOOST_FOREACH(const derived_ptr& child, children_) child->printTree(indent+" "); } /* ************************************************************************* */ - template - void BayesTreeClique::permuteWithInverse(const Permutation& inversePermutation) { + template + void BayesTreeCliqueBase::permuteWithInverse(const Permutation& inversePermutation) { conditional_->permuteWithInverse(inversePermutation); - BOOST_FOREACH(const shared_ptr& child, children_) { + BOOST_FOREACH(const derived_ptr& child, children_) { child->permuteWithInverse(inversePermutation); } assertInvariants(); } /* ************************************************************************* */ - template - bool BayesTreeClique::permuteSeparatorWithInverse(const Permutation& inversePermutation) { + template + bool BayesTreeCliqueBase::permuteSeparatorWithInverse(const Permutation& inversePermutation) { bool changed = conditional_->permuteSeparatorWithInverse(inversePermutation); #ifndef NDEBUG if(!changed) { BOOST_FOREACH(Index& separatorKey, conditional_->parents()) { assert(separatorKey == inversePermutation[separatorKey]); } - BOOST_FOREACH(const shared_ptr& child, children_) { + BOOST_FOREACH(const derived_ptr& child, children_) { assert(child->permuteSeparatorWithInverse(inversePermutation) == false); } } @@ -97,9 +107,8 @@ namespace gtsam { // clique on the root. We can compute it recursively from the parent shortcut // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p /* ************************************************************************* */ - template - BayesNet BayesTreeClique::shortcut(shared_ptr R, - Eliminate function) { + template + BayesNet BayesTreeCliqueBase::shortcut(derived_ptr R, Eliminate function) { static const bool debug = false; @@ -109,16 +118,16 @@ namespace gtsam { shared_ptr parent(parent_.lock()); if (R.get()==this || parent==R) { - BayesNet empty; + BayesNet empty; return empty; } // The root conditional - FactorGraph p_R(BayesNet(R->conditional())); + FactorGraph p_R(BayesNet(R->conditional())); - // The parent clique has a CONDITIONAL for each frontal node in Fp + // The parent clique has a ConditionalType for each frontal node in Fp // so we can obtain P(Fp|Sp) in factor graph form - FactorGraph p_Fp_Sp(BayesNet(parent->conditional())); + FactorGraph p_Fp_Sp(BayesNet(parent->conditional())); // If not the base case, obtain the parent shortcut P(Sp|R) as factors FactorGraph p_Sp_R(parent->shortcut(R, function)); @@ -151,29 +160,29 @@ namespace gtsam { BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) { variablesAtBack.insert(separatorIndex); separator.insert(separatorIndex); - if(debug) cout << "At back (this): " << separatorIndex << endl; + if(debug) std::cout << "At back (this): " << separatorIndex << std::endl; } BOOST_FOREACH(const Index key, R->conditional()->keys()) { if(variablesAtBack.insert(key).second) ++ uniqueRootVariables; - if(debug) cout << "At back (root): " << key << endl; + if(debug) std::cout << "At back (root): " << key << std::endl; } Permutation toBack = Permutation::PushToBack( - vector(variablesAtBack.begin(), variablesAtBack.end()), + std::vector(variablesAtBack.begin(), variablesAtBack.end()), R->conditional()->lastFrontalKey() + 1); Permutation::shared_ptr toBackInverse(toBack.inverse()); BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) { factor->permuteWithInverse(*toBackInverse); } - typename BayesNet::shared_ptr eliminated(EliminationTree< + typename BayesNet::shared_ptr eliminated(EliminationTree< FactorType>::Create(p_Cp_R)->eliminate(function)); // Take only the conditionals for p(S|R). We check for each variable being // in the separator set because if some separator variables overlap with // root variables, we cannot rely on the number of root variables, and also // want to include those variables in the conditional. - BayesNet p_S_R; - BOOST_REVERSE_FOREACH(typename CONDITIONAL::shared_ptr conditional, *eliminated) { + BayesNet p_S_R; + BOOST_REVERSE_FOREACH(typename ConditionalType::shared_ptr conditional, *eliminated) { assert(conditional->nrFrontals() == 1); if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) { if(debug) @@ -199,16 +208,16 @@ namespace gtsam { // \int(Cp\S) P(F|S)P(S|Cp)P(Cp) // Because the root clique could be very big. /* ************************************************************************* */ - template - FactorGraph BayesTreeClique::marginal( - shared_ptr R, Eliminate function) { + template + FactorGraph::FactorType> BayesTreeCliqueBase::marginal( + derived_ptr R, Eliminate function) { // If we are the root, just return this root // NOTE: immediately cast to a factor graph - BayesNet bn(R->conditional()); + BayesNet bn(R->conditional()); if (R.get()==this) return bn; // Combine P(F|S), P(S|R), and P(R) - BayesNet p_FSR = this->shortcut(R, function); + BayesNet p_FSR = this->shortcut(R, function); p_FSR.push_front(this->conditional()); p_FSR.push_back(R->conditional()); @@ -220,9 +229,9 @@ namespace gtsam { /* ************************************************************************* */ // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R) /* ************************************************************************* */ - template - FactorGraph BayesTreeClique::joint( - shared_ptr C2, shared_ptr R, Eliminate function) { + template + FactorGraph::FactorType> BayesTreeCliqueBase::joint( + derived_ptr C2, derived_ptr R, Eliminate function) { // For now, assume neither is the root // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R) @@ -234,14 +243,14 @@ namespace gtsam { joint.push_back(R->conditional()->toFactor()); // P(R) // Find the keys of both C1 and C2 - vector keys1(conditional_->keys()); - vector keys2(C2->conditional_->keys()); + std::vector keys1(conditional_->keys()); + std::vector keys2(C2->conditional_->keys()); FastSet keys12; keys12.insert(keys1.begin(), keys1.end()); keys12.insert(keys2.begin(), keys2.end()); // Calculate the marginal - vector keys12vector; keys12vector.reserve(keys12.size()); + std::vector keys12vector; keys12vector.reserve(keys12.size()); keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end()); assertInvariants(); GenericSequentialSolver solver(joint); diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 83496dfc0..fe7c1b81b 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -24,6 +24,8 @@ #include #include +namespace gtsam { template class BayesTree; } + namespace gtsam { /** @@ -39,9 +41,21 @@ namespace gtsam { * * @tparam The derived clique type. */ - template + template struct BayesTreeCliqueBase { + public: + typedef BayesTreeCliqueBase This; + typedef DERIVED DerivedType; + typedef CONDITIONAL ConditionalType; + typedef boost::shared_ptr sharedConditional; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + typedef boost::shared_ptr derived_ptr; + typedef boost::weak_ptr derived_weak_ptr; + typedef typename ConditionalType::FactorType FactorType; + typedef typename FactorGraph::Eliminate Eliminate; + protected: void assertInvariants() const; @@ -51,18 +65,10 @@ namespace gtsam { /** Construct from a conditional, leaving parent and child pointers uninitialized */ BayesTreeCliqueBase(const sharedConditional& conditional); - public: - typedef BayesTreeClique This; - typedef DERIVED DerivedType; - typedef typename DERIVED::ConditionalType ConditionalType; - typedef boost::shared_ptr sharedConditional; - typedef typename boost::shared_ptr shared_ptr; - typedef typename boost::weak_ptr weak_ptr; - typedef typename boost::shared_ptr derived_ptr; - typedef typename boost::weak_ptr derived_weak_ptr; - typedef typename ConditionalType::FactorType FactorType; - typedef typename FactorGraph::Eliminate Eliminate; + /** Construct from an elimination result, which is a pair */ + BayesTreeCliqueBase(const std::pair >& result); + public: sharedConditional conditional_; derived_weak_ptr parent_; std::list children_; @@ -77,19 +83,7 @@ namespace gtsam { */ static derived_ptr Create(const std::pair >& result) { return boost::make_shared(result); } - void cloneToBayesTree(BayesTree& newTree, shared_ptr parent_clique = shared_ptr()) const { - sharedConditional newConditional = sharedConditional(new CONDITIONAL(*conditional_)); - sharedClique newClique = newTree.addClique(newConditional, parent_clique); - if (cachedFactor_) - newClique->cachedFactor_ = cachedFactor_->clone(); - else newClique->cachedFactor_ = typename FactorType::shared_ptr(); - if (!parent_clique) { - newTree.root_ = newClique; - } - BOOST_FOREACH(const shared_ptr& childClique, children_) { - childClique->cloneToBayesTree(newTree, newClique); - } - } + derived_ptr clone() const { return Create(sharedConditional(new ConditionalType(*conditional_))); } /** print this node */ void print(const std::string& s = "") const; @@ -128,19 +122,21 @@ namespace gtsam { /** return the conditional P(S|Root) on the separator given the root */ // TODO: create a cached version - BayesNet shortcut(shared_ptr root, Eliminate function); + BayesNet shortcut(derived_ptr root, Eliminate function); /** return the marginal P(C) of the clique */ - FactorGraph marginal(shared_ptr root, Eliminate function); + FactorGraph marginal(derived_ptr root, Eliminate function); /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ - FactorGraph joint(shared_ptr C2, shared_ptr root, Eliminate function); + FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function); bool equals(const This& other, double tol=1e-9) const { return (!conditional_ && !other.conditional()) || conditional_->equals(*(other.conditional()), tol); } + friend class BayesTree; + private: /** Serialization function */ friend class boost::serialization::access; @@ -153,8 +149,8 @@ namespace gtsam { }; // \struct Clique - template - typename BayesTreeCliqueBase::derived_ptr asDerived(const BayesTreeCliqueBase& base) { + template + typename BayesTreeCliqueBase::derived_ptr asDerived(const BayesTreeCliqueBase& base) { #ifndef NDEBUG return boost::dynamic_pointer_cast(base); #else @@ -162,6 +158,4 @@ namespace gtsam { #endif } -#include - } diff --git a/gtsam/inference/EliminationTree-inl.h b/gtsam/inference/EliminationTree-inl.h index 1d62e46fc..462a2fd4a 100644 --- a/gtsam/inference/EliminationTree-inl.h +++ b/gtsam/inference/EliminationTree-inl.h @@ -9,7 +9,6 @@ #include #include #include -#include #include #include diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index 6e47983a0..76db131ed 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -10,10 +10,10 @@ #include #include #include -#include #include class EliminationTreeTester; // for unit tests, see testEliminationTree +namespace gtsam { template class BayesNet; } namespace gtsam { @@ -140,3 +140,5 @@ struct DisconnectedGraphException : public std::exception { }; } + +#include diff --git a/gtsam/inference/FactorGraph-inl.h b/gtsam/inference/FactorGraph-inl.h index e776a4390..3b3d568bd 100644 --- a/gtsam/inference/FactorGraph-inl.h +++ b/gtsam/inference/FactorGraph-inl.h @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 645b496ec..3453f14a6 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -239,3 +239,4 @@ template class BayesTree; } // namespace gtsam +#include diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index b4bd22e67..04e8cfbeb 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -18,10 +18,10 @@ #pragma once -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -77,9 +77,8 @@ namespace gtsam { /* ************************************************************************* */ template - typename BayesNet::shared_ptr // - GenericSequentialSolver::eliminate( - typename EliminationTree::Eliminate function) const { + typename boost::shared_ptr > // + GenericSequentialSolver::eliminate(Eliminate function) const { return eliminationTree_->eliminate(function); } @@ -87,8 +86,7 @@ namespace gtsam { template typename FactorGraph::shared_ptr // GenericSequentialSolver::jointFactorGraph( - const std::vector& js, - typename EliminationTree::Eliminate function) const { + const std::vector& js, Eliminate function) const { // Compute a COLAMD permutation with the marginal variable constrained to the end. Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(*structure_, js)); @@ -127,10 +125,9 @@ namespace gtsam { /* ************************************************************************* */ template typename FACTOR::shared_ptr // - GenericSequentialSolver::marginalFactor(Index j, - typename EliminationTree::Eliminate function) const { + GenericSequentialSolver::marginalFactor(Index j, Eliminate function) const { // Create a container for the one variable index - vector js(1); + std::vector js(1); js[0] = j; // Call joint and return the only factor in the factor graph it returns diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h index 16d47f8df..d9c593b25 100644 --- a/gtsam/inference/GenericSequentialSolver.h +++ b/gtsam/inference/GenericSequentialSolver.h @@ -19,9 +19,15 @@ #pragma once #include - +#include +#include +#include #include -#include + +namespace gtsam { class VariableIndex; } +namespace gtsam { template class EliminationTree; } +namespace gtsam { template class FactorGraph; } +namespace gtsam { template class BayesNet; } namespace gtsam { @@ -43,7 +49,12 @@ namespace gtsam { protected: - typedef typename FactorGraph::shared_ptr sharedFactorGraph; + typedef boost::shared_ptr > sharedFactorGraph; + + typedef std::pair< + boost::shared_ptr, + boost::shared_ptr > EliminationResult; + typedef boost::function&, size_t)> Eliminate; /** Store the original factors for computing marginals * TODO Frank says: really? Marginals should be computed from result. @@ -51,15 +62,14 @@ namespace gtsam { sharedFactorGraph factors_; /** Store column structure of the factor graph. Why? */ - VariableIndex::shared_ptr structure_; + boost::shared_ptr structure_; /** Elimination tree that performs elimination */ - typedef EliminationTree EliminationTree_; - typename EliminationTree::shared_ptr eliminationTree_; + boost::shared_ptr > eliminationTree_; /** concept checks */ GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) - GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree_) +// GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree) public: @@ -76,7 +86,7 @@ namespace gtsam { */ GenericSequentialSolver( const sharedFactorGraph& factorGraph, - const VariableIndex::shared_ptr& variableIndex); + const boost::shared_ptr& variableIndex); /** Print to cout */ void print(const std::string& name = "GenericSequentialSolver: ") const; @@ -95,25 +105,23 @@ namespace gtsam { * Eliminate the factor graph sequentially. Uses a column elimination tree * to recursively eliminate. */ - typename BayesNet::shared_ptr - eliminate(typename EliminationTree::Eliminate function) const; + typename boost::shared_ptr > eliminate(Eliminate function) const; /** * Compute the marginal joint over a set of variables, by integrating out * all of the other variables. Returns the result as a factor graph. */ typename FactorGraph::shared_ptr jointFactorGraph( - const std::vector& js, - typename EliminationTree::Eliminate function) const; + const std::vector& js, Eliminate function) const; /** * Compute the marginal Gaussian density over a variable, by integrating out * all of the other variables. This function returns the result as a factor. */ - typename FACTOR::shared_ptr marginalFactor(Index j, - typename EliminationTree::Eliminate function) const; + typename FACTOR::shared_ptr marginalFactor(Index j, Eliminate function) const; }; // GenericSequentialSolver } // namespace gtsam +#include diff --git a/gtsam/inference/ISAM-inl.h b/gtsam/inference/ISAM-inl.h index 8fdad31b2..1daab686f 100644 --- a/gtsam/inference/ISAM-inl.h +++ b/gtsam/inference/ISAM-inl.h @@ -41,7 +41,7 @@ namespace gtsam { // Remove the contaminated part of the Bayes tree BayesNet bn; - removeTop(newFactors.keys(), bn, orphans); + this->removeTop(newFactors.keys(), bn, orphans); FG factors(bn); // add the factors themselves diff --git a/gtsam/inference/JunctionTree-inl.h b/gtsam/inference/JunctionTree-inl.h index b3f5edd49..fca4e2a27 100644 --- a/gtsam/inference/JunctionTree-inl.h +++ b/gtsam/inference/JunctionTree-inl.h @@ -21,11 +21,9 @@ #include #include -#include #include #include -#include -#include +#include #include #include diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h index 12c7dd5d2..2f74357fe 100644 --- a/gtsam/inference/JunctionTree.h +++ b/gtsam/inference/JunctionTree.h @@ -110,3 +110,5 @@ namespace gtsam { }; // JunctionTree } // namespace gtsam + +#include diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp index 5a008a192..2d5190f45 100644 --- a/gtsam/inference/SymbolicFactorGraph.cpp +++ b/gtsam/inference/SymbolicFactorGraph.cpp @@ -18,19 +18,14 @@ #include #include -#include -#include -#include +#include +#include +#include namespace gtsam { using namespace std; - // Explicitly instantiate so we don't have to include everywhere - template class FactorGraph; - template class BayesNet; - template class EliminationTree; - /* ************************************************************************* */ SymbolicFactorGraph::SymbolicFactorGraph(const BayesNet& bayesNet) : FactorGraph(bayesNet) {} diff --git a/gtsam/inference/SymbolicFactorGraph.h b/gtsam/inference/SymbolicFactorGraph.h index dd9932d74..90413ab06 100644 --- a/gtsam/inference/SymbolicFactorGraph.h +++ b/gtsam/inference/SymbolicFactorGraph.h @@ -17,8 +17,13 @@ #pragma once -#include -#include +#include +#include +#include + +namespace gtsam { template class EliminationTree; } +namespace gtsam { template class BayesNet; } +namespace gtsam { class IndexConditional; } namespace gtsam { @@ -72,7 +77,7 @@ namespace gtsam { * Combine and eliminate can also be called separately, but for this and * derived classes calling them separately generally does extra work. */ - std::pair + std::pair, boost::shared_ptr > EliminateSymbolic(const FactorGraph&, size_t nrFrontals = 1); /* Template function implementation */ diff --git a/gtsam/inference/SymbolicMultifrontalSolver.cpp b/gtsam/inference/SymbolicMultifrontalSolver.cpp index 77df4e04d..7348cbc5d 100644 --- a/gtsam/inference/SymbolicMultifrontalSolver.cpp +++ b/gtsam/inference/SymbolicMultifrontalSolver.cpp @@ -16,7 +16,7 @@ */ #include -#include +#include namespace gtsam { diff --git a/gtsam/inference/inference.h b/gtsam/inference/inference.h index 2be90a074..d853ff91b 100644 --- a/gtsam/inference/inference.h +++ b/gtsam/inference/inference.h @@ -18,7 +18,6 @@ #pragma once -#include #include #include @@ -77,4 +76,4 @@ namespace gtsam { return PermutationCOLAMD_(variableIndex, cmember); } -} /// namespace gtsam +} // namespace gtsam diff --git a/gtsam/inference/tests/testClusterTree.cpp b/gtsam/inference/tests/testClusterTree.cpp index ab7716c50..7c7e7dc9a 100644 --- a/gtsam/inference/tests/testClusterTree.cpp +++ b/gtsam/inference/tests/testClusterTree.cpp @@ -27,7 +27,7 @@ using namespace boost::assign; using namespace gtsam; // explicit instantiation and typedef -template class ClusterTree; +namespace gtsam { template class ClusterTree; } typedef ClusterTree SymbolicClusterTree; /* ************************************************************************* */ diff --git a/gtsam/inference/tests/testJunctionTree.cpp b/gtsam/inference/tests/testJunctionTree.cpp index 7dd129472..e35eb8a8f 100644 --- a/gtsam/inference/tests/testJunctionTree.cpp +++ b/gtsam/inference/tests/testJunctionTree.cpp @@ -17,6 +17,7 @@ */ #include // for operator += +#include // for operator += #include // for operator += using namespace boost::assign; @@ -35,6 +36,7 @@ using namespace boost::assign; #include using namespace gtsam; +using namespace std; typedef JunctionTree SymbolicJunctionTree; typedef BayesTree SymbolicBayesTree; diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 2bbfc2f22..8a58bbc34 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -24,7 +24,6 @@ #include #include #include -#include #include // Forward declaration to friend unit tests @@ -39,6 +38,7 @@ namespace gtsam { // Forward declarations class GaussianFactor; +class JacobianFactor; /** * A conditional Gaussian functions as the node in a Bayes network diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index eeee0dccc..ba21eed25 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -22,6 +22,8 @@ #include +namespace gtsam { class SharedDiagonal; } + namespace gtsam { class KalmanFilter { diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index d211b2e06..e11765b6c 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index d873da83c..4ceda0cfb 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -18,6 +18,8 @@ */ #include +#include +#include #include using namespace std; diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2-inl.h similarity index 84% rename from gtsam/nonlinear/GaussianISAM2.cpp rename to gtsam/nonlinear/GaussianISAM2-inl.h index 7df21bf2c..7fcd77873 100644 --- a/gtsam/nonlinear/GaussianISAM2.cpp +++ b/gtsam/nonlinear/GaussianISAM2-inl.h @@ -14,7 +14,8 @@ using namespace gtsam; namespace gtsam { /* ************************************************************************* */ -void optimize2(const BayesTree::sharedClique& clique, double threshold, + template +void optimize2(const boost::shared_ptr& clique, double threshold, vector& changed, const vector& replaced, Permuted& 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 @@ -84,7 +85,7 @@ void optimize2(const BayesTree::sharedClique& clique, doubl } // Recurse to children - BOOST_FOREACH(const BayesTree::sharedClique& child, clique->children_) { + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { optimize2(child, threshold, changed, replaced, delta, count); } } @@ -92,14 +93,15 @@ void optimize2(const BayesTree::sharedClique& clique, doubl /* ************************************************************************* */ // fast full version without threshold -void optimize2(const BayesTree::sharedClique& clique, VectorValues& delta) { + template +void optimize2(const boost::shared_ptr& clique, VectorValues& delta) { // parents are assumed to already be solved and available in result (*clique)->rhs(delta); (*clique)->solveInPlace(delta); // Solve chilren recursively - BOOST_FOREACH(const BayesTree::sharedClique& child, clique->children_) { + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { optimize2(child, delta); } } @@ -114,7 +116,8 @@ void optimize2(const BayesTree::sharedClique& clique, Vecto //} /* ************************************************************************* */ -int optimize2(const BayesTree::sharedClique& root, double threshold, const vector& keys, Permuted& delta) { + template +int optimize2(const boost::shared_ptr& root, double threshold, const vector& keys, Permuted& delta) { vector changed(keys.size(), false); int count = 0; // starting from the root, call optimize on each conditional @@ -123,18 +126,20 @@ int optimize2(const BayesTree::sharedClique& root, double t } /* ************************************************************************* */ -void nnz_internal(const BayesTree::sharedClique& clique, int& result) { + template +void nnz_internal(const boost::shared_ptr& clique, int& result) { int dimR = (*clique)->dim(); int dimSep = (*clique)->get_S().cols() - dimR; result += ((dimR+1)*dimR)/2 + dimSep*dimR; // traverse the children - BOOST_FOREACH(const BayesTree::sharedClique& child, clique->children_) { + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { nnz_internal(child, result); } } /* ************************************************************************* */ -int calculate_nnz(const BayesTree::sharedClique& clique) { + template +int calculate_nnz(const boost::shared_ptr& clique) { int result = 0; // starting from the root, add up entries of frontal and conditional matrices of each conditional nnz_internal(clique, result); diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index fe29936cb..cb6f39865 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -42,7 +42,8 @@ public: }; // optimize the BayesTree, starting from the root -void optimize2(const BayesTree::sharedClique& root, VectorValues& delta); +template +void optimize2(const boost::shared_ptr& root, VectorValues& delta); // optimize the BayesTree, starting from the root; "replaced" needs to contain // all variables that are contained in the top of the Bayes tree that has been @@ -52,10 +53,14 @@ void optimize2(const BayesTree::sharedClique& root, VectorV // and recursive backsubstitution might eventually stop if none of the changed // variables are contained in the subtree. // returns the number of variables that were solved for -int optimize2(const BayesTree::sharedClique& root, +template +int optimize2(const boost::shared_ptr& root, double threshold, const std::vector& replaced, Permuted& delta); // calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) -int calculate_nnz(const BayesTree::sharedClique& clique); +template +int calculate_nnz(const boost::shared_ptr& clique); }/// namespace gtsam + +#include diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index d414e13e9..85b28d744 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -81,7 +81,7 @@ struct ISAM2::Impl { * * Alternatively could we trace up towards the root for each variable here? */ - static void FindAll(typename BayesTreeClique::shared_ptr clique, FastSet& keys, const vector& markedMask); + static void FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask); /** * Apply expmap to the given values, but only for indices appearing in @@ -148,7 +148,7 @@ void ISAM2::Impl::AddVariables( const size_t originalDim = delta->dim(); const size_t originalnVars = delta->size(); delta.container().append(dims); - delta.container().vector().segment(originalDim, newDim) = Vector::Zero(newDim); + delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); delta.permutation().resize(originalnVars + newTheta.size()); { _VariableAdder vadder(ordering, delta, originalnVars); @@ -188,7 +188,7 @@ FastSet ISAM2::Impl::CheckRelinearization(Permu /* ************************************************************************* */ template -void ISAM2::Impl::FindAll(typename BayesTreeClique::shared_ptr clique, FastSet& keys, const vector& markedMask) { +void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { static const bool debug = false; // does the separator contain any of the variables? bool found = false; @@ -202,7 +202,7 @@ void ISAM2::Impl::FindAll(typename BayesTreeCliqueprint("Key(s) marked in clique "); if(debug) cout << "so marking key " << (*clique)->keys().front() << endl; } - BOOST_FOREACH(const typename BayesTreeClique::shared_ptr& child, clique->children_) { + BOOST_FOREACH(const typename ISAM2Clique::shared_ptr& child, clique->children_) { FindAll(child, keys, markedMask); } } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 4ed103615..5d2984c9c 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -286,7 +286,7 @@ boost::shared_ptr > ISAM2::recalculat toc(5,"eliminate"); tic(6,"insert"); - BayesTree::clear(); + this->clear(); this->insert(newRoot); toc(6,"insert"); @@ -310,6 +310,7 @@ boost::shared_ptr > ISAM2::recalculat affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end()); tic(1,"relinearizeAffected"); GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys)); + if(debug) factors.print("Relinearized factors: "); toc(1,"relinearizeAffected"); if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index f3e960db4..8663aae41 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -106,21 +106,18 @@ struct ISAM2Result { }; template -struct ISAM2Clique : public BayesTreeCliqueBase > { +struct ISAM2Clique : public BayesTreeCliqueBase, CONDITIONAL> { typedef ISAM2Clique This; - typedef BayesTreeCliqueBase Base; + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + typedef CONDITIONAL ConditionalType; + typedef typename ConditionalType::shared_ptr sharedConditional; typename Base::FactorType::shared_ptr cachedFactor_; Vector gradientContribution_; - /** Access the cached factor */ - typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } - - /** Access the gradient contribution */ - const Vector& gradientContribution() const { return gradientContribution_; } - /** Construct from a conditional */ ISAM2Clique(const sharedConditional& conditional) : Base(conditional) { throw runtime_error("ISAM2Clique should always be constructed with the elimination result constructor"); } @@ -130,8 +127,33 @@ struct ISAM2Clique : public BayesTreeCliqueBase > { Base(result.first), cachedFactor_(result.second), gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) { // Compute gradient contribution const ConditionalType& conditional(*result.first); - gradient << -(conditional.get_R() * conditional.permutation().transpose()).transpose * conditional.get_d(), - -conditional.get_S() * conditional.get_d(); + gradientContribution_ << -(conditional.get_R() * conditional.permutation().transpose()).transpose() * conditional.get_d(), + -conditional.get_S().transpose() * conditional.get_d(); + } + + /** Produce a deep copy, copying the cached factor and gradient contribution */ + shared_ptr clone() const { + shared_ptr copy(new ISAM2Clique(make_pair( + sharedConditional(new ConditionalType(*Base::conditional_)), + cachedFactor_ ? cachedFactor_->clone() : typename Base::FactorType::shared_ptr()))); + copy->gradientContribution_ = gradientContribution_; + return copy; + } + + /** Access the cached factor */ + typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } + + /** Access the gradient contribution */ + const Vector& gradientContribution() const { return gradientContribution_; } + + void permuteWithInverse(const Permutation& inversePermutation) { + if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); + Base::permuteWithInverse(inversePermutation); + } + + bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { + if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); + return Base::permuteSeparatorWithInverse(inversePermutation); } private: @@ -214,7 +236,7 @@ public: typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class - void cloneTo(boost::shared_ptr& newISAM2) const { + void cloneTo(boost::shared_ptr& newISAM2) const { boost::shared_ptr bayesTree = boost::static_pointer_cast(newISAM2); Base::cloneTo(bayesTree); newISAM2->theta_ = theta_; diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index 610bb7f5c..b944b105d 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -30,7 +30,7 @@ headers += DoglegOptimizer.h DoglegOptimizer-inl.h # Nonlinear iSAM(2) headers += NonlinearISAM.h NonlinearISAM-inl.h headers += ISAM2.h ISAM2-inl.h ISAM2-impl-inl.h -sources += GaussianISAM2.cpp +headers += GaussianISAM2.h GaussianISAM2-inl.h # Nonlinear constraints headers += NonlinearEquality.h diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ad22d3e41..c65b4ac08 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -249,7 +249,7 @@ public: * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. */ virtual double error(const VALUES& c) const { - if (active(c)) + if (this->active(c)) return 0.5 * noiseModel_->distance(unwhitenedError(c)); else return 0.0; @@ -262,7 +262,7 @@ public: */ boost::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { // Only linearize if the factor is active - if (!active(x)) + if (!this->active(x)) return boost::shared_ptr(); // Create the set of terms - Jacobians for each index diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index 79a9aa436..faa06bbc7 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include #include diff --git a/gtsam/nonlinear/tests/testOrdering.cpp b/gtsam/nonlinear/tests/testOrdering.cpp index 64f5e7f4c..8c1804016 100644 --- a/gtsam/nonlinear/tests/testOrdering.cpp +++ b/gtsam/nonlinear/tests/testOrdering.cpp @@ -15,6 +15,7 @@ */ #include +#include #include using namespace gtsam; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 121717cf3..8bdacbf0f 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -167,7 +167,8 @@ TEST(ISAM2, optimize2) { conditional->solveInPlace(expected); // Clique - GaussianISAM2::sharedClique clique(GaussianISAM2::Clique::Create(conditional)); + GaussianISAM2::sharedClique clique( + GaussianISAM2::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr()))); VectorValues actual(theta.dims(ordering)); conditional->rhs(actual); optimize2(clique, actual); @@ -192,9 +193,13 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const planarSLAM::Values& fu } /* ************************************************************************* */ -TEST(ISAM2, slamlike_solution) +TEST_UNSAFE(ISAM2, slamlike_solution) { +// SETDEBUG("ISAM2 update", true); +// SETDEBUG("ISAM2 update verbose", true); +// SETDEBUG("ISAM2 recalculate", true); + // Pose and landmark key types from planarSLAM typedef planarSLAM::PoseKey PoseKey; typedef planarSLAM::PointKey PointKey; From ef2021f6da326bc19194cdc5e71c0a4b9aab14f8 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 13 Dec 2011 18:40:31 +0000 Subject: [PATCH 08/17] (in branch) fixed filename capitalization problem during merge --- gtsam/slam/Simulated3D.cpp | 43 ------------ gtsam/slam/Simulated3D.h | 132 ------------------------------------- 2 files changed, 175 deletions(-) delete mode 100644 gtsam/slam/Simulated3D.cpp delete mode 100644 gtsam/slam/Simulated3D.h diff --git a/gtsam/slam/Simulated3D.cpp b/gtsam/slam/Simulated3D.cpp deleted file mode 100644 index e2f6211a5..000000000 --- a/gtsam/slam/Simulated3D.cpp +++ /dev/null @@ -1,43 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated3D.cpp -* @brief measurement functions and derivatives for simulated 3D robot -* @author Alex Cunningham -**/ - -#include - -namespace gtsam { - -namespace simulated3D { - -Point3 prior (const Point3& x, boost::optional H) { - if (H) *H = eye(3); - return x; -} - -Point3 odo(const Point3& x1, const Point3& x2, - boost::optional H1, boost::optional H2) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); - return x2 - x1; -} - -Point3 mea(const Point3& x, const Point3& l, - boost::optional H1, boost::optional H2) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); - return l - x; -} - -}} // namespace gtsam::simulated3D diff --git a/gtsam/slam/Simulated3D.h b/gtsam/slam/Simulated3D.h deleted file mode 100644 index 32f05e967..000000000 --- a/gtsam/slam/Simulated3D.h +++ /dev/null @@ -1,132 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 Simulated3D.h - * @brief measurement functions and derivatives for simulated 3D robot - * @author Alex Cunningham - **/ - -// \callgraph - -#pragma once - -#include -#include -#include -#include -#include -#include - -// \namespace - -namespace gtsam { -namespace simulated3D { - -/** - * This is a linear SLAM domain where both poses and landmarks are - * 3D points, without rotation. The structure and use is based on - * the simulated2D domain. - */ - -typedef gtsam::TypedSymbol PoseKey; -typedef gtsam::TypedSymbol PointKey; - -typedef Values PoseValues; -typedef Values PointValues; -typedef TupleValues2 Values; - -/** - * Prior on a single pose - */ -Point3 prior(const Point3& x, boost::optional H = boost::none); - -/** - * odometry between two poses - */ -Point3 odo(const Point3& x1, const Point3& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none); - -/** - * measurement between landmark and pose - */ -Point3 mea(const Point3& x, const Point3& l, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none); - -/** - * A prior factor on a single linear robot pose - */ -struct PointPrior3D: public NonlinearFactor1 { - - Point3 z_; ///< The prior pose value for the variable attached to this factor - - /** - * Constructor for a prior factor - * @param z is the measured/prior position for the pose - * @param model is the measurement model for the factor (Dimension: 3) - * @param j is the key for the pose - */ - PointPrior3D(const Point3& z, - const SharedNoiseModel& model, const PoseKey& j) : - NonlinearFactor1 (model, j), z_(z) { - } - - /** - * Evaluates the error at a given value of x, - * with optional derivatives. - * @param x is the current value of the variable - * @param H is an optional Jacobian matrix (Dimension: 3x3) - * @return Vector error between prior value and x (Dimension: 3) - */ - Vector evaluateError(const Point3& x, boost::optional H = - boost::none) { - return (prior(x, H) - z_).vector(); - } -}; - -/** - * Models a linear 3D measurement between 3D points - */ -struct Simulated3DMeasurement: public NonlinearFactor2 { - - Point3 z_; ///< Linear displacement between a pose and landmark - - /** - * Creates a measurement factor with a given measurement - * @param z is the measurement, a linear displacement between poses and landmarks - * @param model is a measurement model for the factor (Dimension: 3) - * @param j1 is the pose key of the robot - * @param j2 is the point key for the landmark - */ - Simulated3DMeasurement(const Point3& z, - const SharedNoiseModel& model, PoseKey& j1, PointKey j2) : - NonlinearFactor2 ( - model, j1, j2), z_(z) { - } - - /** - * Error function with optional derivatives - * @param x1 a robot pose value - * @param x2 a landmark point value - * @param H1 is an optional Jacobian matrix in terms of x1 (Dimension: 3x3) - * @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3) - * @return vector error between measurement and prediction (Dimension: 3) - */ - Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional< - Matrix&> H1 = boost::none, boost::optional H2 = boost::none) { - return (mea(x1, x2, H1, H2) - z_).vector(); - } -}; - -}} // namespace simulated3D From e75e4321af8552e559954f044c8997a9e84b4941 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 13 Dec 2011 18:45:21 +0000 Subject: [PATCH 09/17] (in branch) slight simplification of HessianFactor eliminate code - fill in keys when the combined factor is first constructed so it is a complete factor --- gtsam/linear/GaussianFactorGraph.cpp | 8 ++------ gtsam/linear/HessianFactor.cpp | 12 +++++++++--- gtsam/linear/HessianFactor.h | 2 +- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 3ace299cb..98e74abd7 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -367,10 +367,8 @@ namespace gtsam { // Pull out keys and dimensions tic(2, "keys"); - vector keys(scatter.size()); vector dimensions(scatter.size() + 1); BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { - keys[var_slot.second.slot] = var_slot.first; dimensions[var_slot.second.slot] = var_slot.second.dimension; } // This is for the r.h.s. vector @@ -401,7 +399,7 @@ namespace gtsam { // Extract conditionals and fill in details of the remaining factor tic(5, "split"); GaussianConditional::shared_ptr conditionals = - combinedFactor->splitEliminatedFactor(nrFrontals, keys); + combinedFactor->splitEliminatedFactor(nrFrontals); if (debug) { conditionals->print("Extracted conditionals: "); combinedFactor->print("Eliminated factor (L piece): "); @@ -572,10 +570,8 @@ namespace gtsam { // Pull out keys and dimensions tic(2, "keys"); - vector keys(scatter.size()); vector dimensions(scatter.size() + 1); BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { - keys[var_slot.second.slot] = var_slot.first; dimensions[var_slot.second.slot] = var_slot.second.dimension; } // This is for the r.h.s. vector @@ -609,7 +605,7 @@ namespace gtsam { // Extract conditionals and fill in details of the remaining factor tic(5, "split"); GaussianConditional::shared_ptr conditionals = - combinedFactor->splitEliminatedFactor(nrFrontals, keys, permutation); + combinedFactor->splitEliminatedFactor(nrFrontals, permutation); if (debug) { conditionals->print("Extracted conditionals: "); combinedFactor->print("Eliminated factor (L piece): "); diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index da57e302b..5cba4f4dc 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -193,6 +194,9 @@ HessianFactor::HessianFactor(const FactorGraph& factors, // Form Ab' * Ab tic(1, "allocate"); info_.resize(dimensions.begin(), dimensions.end(), false); + // Fill in keys + keys_.resize(scatter.size()); + std::transform(scatter.begin(), scatter.end(), keys_.begin(), boost::bind(&Scatter::value_type::first, ::_1)); toc(1, "allocate"); tic(2, "zero"); matrix_.noalias() = Matrix::Zero(matrix_.rows(),matrix_.cols()); @@ -406,7 +410,7 @@ Eigen::LDLT::TranspositionType HessianFactor::partialLDL(size_t nrFronta /* ************************************************************************* */ GaussianConditional::shared_ptr -HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector& keys, const Eigen::LDLT::TranspositionType& permutation) { +HessianFactor::splitEliminatedFactor(size_t nrFrontals, const Eigen::LDLT::TranspositionType& permutation) { static const bool debug = false; @@ -423,7 +427,7 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector& key // Because of the pivoting permutation when using LDL, treating each variable separately doesn't make sense. tic(2, "construct cond"); Vector sigmas = Vector::Ones(varDim); - conditionals = boost::make_shared(keys.begin(), keys.end(), nrFrontals, Ab, sigmas, permutation); + conditionals = boost::make_shared(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas, permutation); toc(2, "construct cond"); if(debug) conditionals->print("Extracted conditional: "); @@ -433,7 +437,9 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector& key tic(2, "remaining factor"); info_.blockStart() = nrFrontals; // Assign the keys - keys_.assign(keys.begin() + nrFrontals, keys.end()); + vector remainingKeys(keys_.size() - nrFrontals); + remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end()); + keys_.swap(remainingKeys); toc(2, "remaining factor"); return conditionals; diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 88e4364ae..2af208a59 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -276,7 +276,7 @@ namespace gtsam { /** split partially eliminated factor */ boost::shared_ptr splitEliminatedFactor( - size_t nrFrontals, const std::vector& keys, const Eigen::LDLT::TranspositionType& permutation = Eigen::LDLT::TranspositionType()); + size_t nrFrontals, const Eigen::LDLT::TranspositionType& permutation = Eigen::LDLT::TranspositionType()); /** assert invariants */ void assertInvariants() const; From 88c3e81a7df85ecd37ca81c38c2d8e91505bebb9 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 13 Dec 2011 18:46:31 +0000 Subject: [PATCH 10/17] (in branch) bug fix and unit test in permutation bug introduced during BayesTree Clique refactoring --- gtsam/inference/BayesTree-inl.h | 2 +- gtsam/inference/BayesTreeCliqueBase-inl.h | 6 +-- gtsam/inference/BayesTreeCliqueBase.h | 15 +++--- gtsam/linear/GaussianConditional.cpp | 2 +- gtsam/nonlinear/ISAM2-impl-inl.h | 2 + gtsam/nonlinear/ISAM2.h | 16 +++++- tests/testGaussianISAM2.cpp | 62 +++++++++++++++++++++++ 7 files changed, 91 insertions(+), 14 deletions(-) diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 7f77fa898..1c696d3ef 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -284,7 +284,7 @@ namespace gtsam { const typename BayesTree::sharedClique& v1, const typename BayesTree::sharedClique& v2 ) { - return v1->equals(*v2); + return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2)); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index d2d2cec65..dfc8864f7 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -66,7 +66,7 @@ namespace gtsam { /* ************************************************************************* */ template void BayesTreeCliqueBase::printTree(const std::string& indent) const { - print(indent); + asDerived(this)->print(indent); BOOST_FOREACH(const derived_ptr& child, children_) child->printTree(indent+" "); } @@ -94,7 +94,7 @@ namespace gtsam { } #endif if(changed) { - BOOST_FOREACH(const shared_ptr& child, children_) { + BOOST_FOREACH(const derived_ptr& child, children_) { (void)child->permuteSeparatorWithInverse(inversePermutation); } } @@ -115,7 +115,7 @@ namespace gtsam { // A first base case is when this clique or its parent is the root, // in which case we return an empty Bayes net. - shared_ptr parent(parent_.lock()); + derived_ptr parent(parent_.lock()); if (R.get()==this || parent==R) { BayesNet empty; diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index fe7c1b81b..66be72f86 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -132,7 +132,7 @@ namespace gtsam { bool equals(const This& other, double tol=1e-9) const { return (!conditional_ && !other.conditional()) || - conditional_->equals(*(other.conditional()), tol); + conditional_->equals(*other.conditional(), tol); } friend class BayesTree; @@ -150,12 +150,13 @@ namespace gtsam { }; // \struct Clique template - typename BayesTreeCliqueBase::derived_ptr asDerived(const BayesTreeCliqueBase& base) { -#ifndef NDEBUG - return boost::dynamic_pointer_cast(base); -#else - return boost::static_pointer_cast(base); -#endif + const DERIVED* asDerived(const BayesTreeCliqueBase* base) { + return static_cast(base); + } + + template + DERIVED* asDerived(BayesTreeCliqueBase* base) { + return static_cast(base); } } diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 37d9c1add..168bfec97 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -162,7 +162,7 @@ void GaussianConditional::print(const string &s) const } gtsam::print(Vector(get_d()),"d"); gtsam::print(sigmas_,"sigmas"); - cout << "Permutation: " << permutation_.indices() << endl; + cout << "Permutation: " << permutation_.indices().transpose() << endl; } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index 85b28d744..17c24e23a 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -337,6 +337,8 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& facto JunctionTree jt(factors, affectedFactorsIndex); result.bayesTree = jt.eliminate(EliminatePreferLDL); if(debug && result.bayesTree) { + if(boost::dynamic_pointer_cast >(result.bayesTree)) + cout << "Is an ISAM2 clique" << endl; cout << "Re-eliminated BT:\n"; result.bayesTree->printTree(""); } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 8663aae41..44df3035f 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -146,14 +146,26 @@ struct ISAM2Clique : public BayesTreeCliqueBase, CONDIT /** Access the gradient contribution */ const Vector& gradientContribution() const { return gradientContribution_; } + bool equals(const This& other, double tol=1e-9) const { + return Base::equals(other) && ((!cachedFactor_ && !other.cachedFactor_) || (cachedFactor_ && other.cachedFactor_ && cachedFactor_->equals(*other.cachedFactor_, tol))); + } + + /** print this node */ + void print(const std::string& s = "") const { + Base::print(s); + if(cachedFactor_) cachedFactor_->print(s + "Cached: "); + else cout << s << "Cached empty" << endl; + } + void permuteWithInverse(const Permutation& inversePermutation) { if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); Base::permuteWithInverse(inversePermutation); } bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { - if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); - return Base::permuteSeparatorWithInverse(inversePermutation); + bool changed = Base::permuteSeparatorWithInverse(inversePermutation); + if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); + return changed; } private: diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 8bdacbf0f..d56d2edbc 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -6,6 +6,7 @@ #include #include // for operator += +#include using namespace boost::assign; #include @@ -401,6 +402,67 @@ TEST_UNSAFE(ISAM2, clone) { CHECK(assert_equal(isam, *isam2)); } +/* ************************************************************************* */ +TEST(ISAM2, permute_cached) { + typedef ISAM2Clique Clique; + typedef boost::shared_ptr > sharedClique; + + // Construct expected permuted BayesTree (variable 2 has been changed to 1) + BayesTree expected; + expected.insert(sharedClique(new Clique(make_pair( + boost::make_shared(pair_list_of + (3, Matrix_(1,1,1.0)) + (4, Matrix_(1,1,2.0)), + 2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4) + HessianFactor::shared_ptr())))); // Cached: empty + expected.insert(sharedClique(new Clique(make_pair( + boost::make_shared(pair_list_of + (2, Matrix_(1,1,1.0)) + (3, Matrix_(1,1,2.0)), + 1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3) + boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) + expected.insert(sharedClique(new Clique(make_pair( + boost::make_shared(pair_list_of + (0, Matrix_(1,1,1.0)) + (2, Matrix_(1,1,2.0)), + 1, Vector_(1,1.0), Vector_(1,1.0)), // p(0|2) + boost::make_shared(1, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(1) + // Change variable 2 to 1 + expected.root()->children().front()->conditional()->keys()[0] = 1; + expected.root()->children().front()->children().front()->conditional()->keys()[1] = 1; + + // Construct unpermuted BayesTree + BayesTree actual; + actual.insert(sharedClique(new Clique(make_pair( + boost::make_shared(pair_list_of + (3, Matrix_(1,1,1.0)) + (4, Matrix_(1,1,2.0)), + 2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4) + HessianFactor::shared_ptr())))); // Cached: empty + actual.insert(sharedClique(new Clique(make_pair( + boost::make_shared(pair_list_of + (2, Matrix_(1,1,1.0)) + (3, Matrix_(1,1,2.0)), + 1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3) + boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) + actual.insert(sharedClique(new Clique(make_pair( + boost::make_shared(pair_list_of + (0, Matrix_(1,1,1.0)) + (2, Matrix_(1,1,2.0)), + 1, Vector_(1,1.0), Vector_(1,1.0)), // p(0|2) + boost::make_shared(2, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(2) + + // Create permutation that changes variable 2 -> 0 + Permutation permutation = Permutation::Identity(5); + permutation[2] = 1; + + // Permute BayesTree + actual.root()->permuteWithInverse(permutation); + + // Check + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From a56fe9572e04dbe3462d53b59a5b1b214dfb5bd2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 13 Dec 2011 22:48:50 +0000 Subject: [PATCH 11/17] (in branch) comment fix --- gtsam/inference/FactorGraph.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 3453f14a6..df684d7d2 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -77,7 +77,7 @@ template class BayesTree; template FactorGraph(const BayesNet& bayesNet); - /** convert from Bayes net */ + /** convert from Bayes tree */ template FactorGraph(const BayesTree& bayesTree); From 2ef911b7e9a6e8d00e0094c079cb01ea0892bee5 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 13 Dec 2011 22:49:37 +0000 Subject: [PATCH 12/17] (in branch) added VectorValues::setZero() function --- gtsam/linear/VectorValues.cpp | 9 +++++++-- gtsam/linear/VectorValues.h | 5 ++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index d592ccdfe..6c3ec797e 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -39,7 +39,7 @@ VectorValues& VectorValues::operator=(const VectorValues& rhs) { /* ************************************************************************* */ VectorValues VectorValues::Zero(const VectorValues& x) { VectorValues cloned(SameStructure(x)); - cloned.vector() = Vector::Zero(x.dim()); + cloned.setZero(); return cloned; } @@ -124,10 +124,15 @@ VectorValues VectorValues::SameStructure(const VectorValues& other) { /* ************************************************************************* */ VectorValues VectorValues::Zero(Index nVars, size_t varDim) { VectorValues ret(nVars, varDim); - ret.vector() = Vector::Zero(ret.dim()); + ret.setZero(); return ret; } +/* ************************************************************************* */ +void VectorValues::setZero() { + values_.setZero(); +} + /* ************************************************************************* */ bool VectorValues::hasSameStructure(const VectorValues& other) const { if(this->size() != other.size()) diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 0bcafd23c..33351ceb7 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -238,6 +238,9 @@ namespace gtsam { template void append(const CONTAINER& dimensions); + /** Set all entries to zero, does not modify the size. */ + void setZero(); + /** Reference the entire solution vector (const version). */ const Vector& vector() const { chk(); return values_; } @@ -383,7 +386,7 @@ namespace gtsam { template VectorValues VectorValues::Zero(const CONTAINER& dimensions) { VectorValues ret(dimensions); - ret.vector() = Vector::Zero(ret.dim()); + ret.setZero(); return ret; } From 6e1136ba20dc2cf4e4586773fe7deb2356191340 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 13 Dec 2011 22:51:28 +0000 Subject: [PATCH 13/17] (in branch) Separate gradient functions for FactorGraph, BayesNet, BayesTree, and BayesTree with ISAM2 Cliques (specialized for using stored gradient contributions in ISAM2 cliques) --- gtsam/linear/GaussianBayesNet.cpp | 11 ++ gtsam/linear/GaussianBayesNet.h | 28 ++- gtsam/linear/JacobianFactor.cpp | 17 +- gtsam/linear/JacobianFactor.h | 23 ++- gtsam/nonlinear/DoglegOptimizerImpl.h | 10 +- gtsam/nonlinear/GaussianISAM2-inl.h | 236 ++++++++++++++------------ gtsam/nonlinear/GaussianISAM2.cpp | 61 +++++++ gtsam/nonlinear/GaussianISAM2.h | 79 +++++++-- gtsam/nonlinear/Makefile.am | 3 +- 9 files changed, 330 insertions(+), 138 deletions(-) create mode 100644 gtsam/nonlinear/GaussianISAM2.cpp diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 9806b435c..e05fbe45c 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -213,6 +214,16 @@ double determinant(const GaussianBayesNet& bayesNet) { return exp(logDet); } +/* ************************************************************************* */ +VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) { + return gradient(FactorGraph(bayesNet), x0); +} + +/* ************************************************************************* */ +void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) { + gradientAtZero(FactorGraph(bayesNet), g); +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 171d061ee..455536a4f 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -64,19 +64,19 @@ namespace gtsam { */ boost::shared_ptr optimize_(const GaussianBayesNet& bn); - /* + /** * Backsubstitute * (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) * @param y is the RHS of the system */ VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& y); - /* + /** * Backsubstitute in place, y starts as RHS and is replaced with solution */ void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y); - /* + /** * Transpose Backsubstitute * gy=inv(L)*gx by solving L*gy=gx. * gy=inv(R'*inv(Sigma))*gx @@ -108,4 +108,26 @@ namespace gtsam { */ double determinant(const GaussianBayesNet& bayesNet); + /** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ R^T(Rx-d) \f$. + * @param bayesNet The Gaussian Bayes net $(R,d)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ + VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0); + + /** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around zero. + * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). + * @param bayesNet The Gaussian Bayes net $(R,d)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ + void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g); + } /// namespace gtsam diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index c20dab3c1..3347fa4f9 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -582,17 +582,28 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValues gradient(const FactorGraph& fg, const VectorValues& x) { + VectorValues gradient(const FactorGraph& fg, const VectorValues& x0) { // It is crucial for performance to make a zero-valued clone of x - VectorValues g = VectorValues::Zero(x); + VectorValues g = VectorValues::Zero(x0); Errors e; BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { - e.push_back(factor->error_vector(x)); + e.push_back(factor->error_vector(x0)); } transposeMultiplyAdd(fg, 1.0, e, g); return g; } + /* ************************************************************************* */ + void gradientAtZero(const FactorGraph& fg, VectorValues& g) { + // Zero-out the gradient + g.setZero(); + Errors e; + BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { + e.push_back(-factor->getb()); + } + transposeMultiplyAdd(fg, 1.0, e, g); + } + /* ************************************************************************* */ void residual(const FactorGraph& fg, const VectorValues &x, VectorValues &r) { Index i = 0 ; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 09089df20..35a1bfa9d 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -318,11 +318,26 @@ namespace gtsam { void transposeMultiplyAdd(const FactorGraph& fg, double alpha, const Errors& e, VectorValues& x); /** - * Calculate Gradient of A^(A*x-b) for a given config - * @param x: VectorValues specifying where to calculate gradient - * @return gradient, as a VectorValues as well + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ A^T(Ax-b) \f$. + * @param fg The Jacobian factor graph $(A,b)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues */ - VectorValues gradient(const FactorGraph& fg, const VectorValues& x); + VectorValues gradient(const FactorGraph& fg, const VectorValues& x0); + + /** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, + * centered around zero. + * The gradient is \f$ A^T(Ax-b) \f$. + * @param fg The Jacobian factor graph $(A,b)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ + void gradientAtZero(const FactorGraph& fg, VectorValues& g); // matrix-vector operations void residual(const FactorGraph& fg, const VectorValues &x, VectorValues &r); diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 5f5801918..28b4185ca 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -233,14 +233,14 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( template VectorValues DoglegOptimizerImpl::ComputeSteepestDescentPoint(const M& Rd) { - // Compute gradient - // Convert to JacobianFactor's to use existing gradient function - FactorGraph jfg(Rd); - VectorValues grad = gradient(jfg, VectorValues::Zero(*allocateVectorValues(Rd))); + // Compute gradient (call gradientAtZero function, which is defined for various linear systems) + VectorValues grad = *allocateVectorValues(Rd); + gradientAtZero(Rd, grad); double gradientSqNorm = grad.dot(grad); // Compute R * g - Errors Rg = jfg * grad; + FactorGraph Rd_jfg(Rd); + Errors Rg = Rd_jfg * grad; // Compute minimizing step size double step = -gradientSqNorm / dot(Rg, Rg); diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h index 7fcd77873..d0d37d886 100644 --- a/gtsam/nonlinear/GaussianISAM2-inl.h +++ b/gtsam/nonlinear/GaussianISAM2-inl.h @@ -1,10 +1,22 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianISAM2 * @brief Full non-linear ISAM * @author Michael Kaess */ -#include +#include +#include using namespace std; using namespace gtsam; @@ -13,137 +25,137 @@ using namespace gtsam; namespace gtsam { -/* ************************************************************************* */ + /* ************************************************************************* */ template -void optimize2(const boost::shared_ptr& clique, double threshold, - vector& changed, const vector& replaced, Permuted& 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 + void optimize2(const boost::shared_ptr& clique, double threshold, + vector& changed, const vector& replaced, Permuted& 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 - // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced[(*clique)->frontals().front()]; + // Are any clique variables part of the tree that has been redone? + bool cliqueReplaced = replaced[(*clique)->frontals().front()]; #ifndef NDEBUG - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - assert(cliqueReplaced == replaced[frontal]); - } + BOOST_FOREACH(Index frontal, (*clique)->frontals()) { + assert(cliqueReplaced == replaced[frontal]); + } #endif - // If not redone, then has one of the separator variables changed significantly? - bool recalculate = cliqueReplaced; - if(!recalculate) { - BOOST_FOREACH(Index parent, (*clique)->parents()) { - if(changed[parent]) { - recalculate = true; - break; - } - } - } - - // Solve clique if it was replaced, or if any parents were changed above the - // threshold or themselves replaced. - if(recalculate) { - - // Temporary copy of the original values, to check how much they change - vector originalValues((*clique)->nrFrontals()); - GaussianConditional::const_iterator it; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - originalValues[it - (*clique)->beginFrontals()] = delta[*it]; - } - - // Back-substitute - (*clique)->rhs(delta); - (*clique)->solveInPlace(delta); - count += (*clique)->nrFrontals(); - - // Whether the values changed above a threshold, or always true if the - // clique was replaced. - bool valuesChanged = cliqueReplaced; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - if(!valuesChanged) { - const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]); - const SubVector& newValue(delta[*it]); - if((oldValue - newValue).lpNorm() >= threshold) { - valuesChanged = true; + // If not redone, then has one of the separator variables changed significantly? + bool recalculate = cliqueReplaced; + if(!recalculate) { + BOOST_FOREACH(Index parent, (*clique)->parents()) { + if(changed[parent]) { + recalculate = true; break; } - } else - break; + } } - // If the values were above the threshold or this clique was replaced - if(valuesChanged) { - // Set changed flag for each frontal variable and leave the new values - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - changed[frontal] = true; - } - } else { - // Replace with the old values + // Solve clique if it was replaced, or if any parents were changed above the + // threshold or themselves replaced. + if(recalculate) { + + // Temporary copy of the original values, to check how much they change + vector originalValues((*clique)->nrFrontals()); + GaussianConditional::const_iterator it; for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - delta[*it] = originalValues[it - (*clique)->beginFrontals()]; + originalValues[it - (*clique)->beginFrontals()] = delta[*it]; + } + + // Back-substitute + (*clique)->rhs(delta); + (*clique)->solveInPlace(delta); + count += (*clique)->nrFrontals(); + + // Whether the values changed above a threshold, or always true if the + // clique was replaced. + bool valuesChanged = cliqueReplaced; + for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + if(!valuesChanged) { + const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]); + const SubVector& newValue(delta[*it]); + if((oldValue - newValue).lpNorm() >= threshold) { + valuesChanged = true; + break; + } + } else + break; + } + + // If the values were above the threshold or this clique was replaced + if(valuesChanged) { + // Set changed flag for each frontal variable and leave the new values + BOOST_FOREACH(Index frontal, (*clique)->frontals()) { + changed[frontal] = true; + } + } else { + // Replace with the old values + for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + delta[*it] = originalValues[it - (*clique)->beginFrontals()]; + } + } + + // Recurse to children + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { + optimize2(child, threshold, changed, replaced, delta, count); } } + } - // Recurse to children + /* ************************************************************************* */ + // fast full version without threshold + template + void optimize2(const boost::shared_ptr& clique, VectorValues& delta) { + + // parents are assumed to already be solved and available in result + (*clique)->rhs(delta); + (*clique)->solveInPlace(delta); + + // Solve chilren recursively BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { - optimize2(child, threshold, changed, replaced, delta, count); + optimize2(child, delta); } - } -} + } -/* ************************************************************************* */ -// fast full version without threshold + ///* ************************************************************************* */ + //boost::shared_ptr optimize2(const GaussianISAM2::sharedClique& root) { + // boost::shared_ptr delta(new VectorValues()); + // set changed; + // // starting from the root, call optimize on each conditional + // optimize2(root, delta); + // return delta; + //} + + /* ************************************************************************* */ template -void optimize2(const boost::shared_ptr& clique, VectorValues& delta) { + int optimize2(const boost::shared_ptr& root, double threshold, const vector& keys, Permuted& delta) { + vector changed(keys.size(), false); + int count = 0; + // starting from the root, call optimize on each conditional + optimize2(root, threshold, changed, keys, delta, count); + return count; + } - // parents are assumed to already be solved and available in result - (*clique)->rhs(delta); - (*clique)->solveInPlace(delta); - - // Solve chilren recursively - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { - optimize2(child, delta); - } -} - -///* ************************************************************************* */ -//boost::shared_ptr optimize2(const GaussianISAM2::sharedClique& root) { -// boost::shared_ptr delta(new VectorValues()); -// set changed; -// // starting from the root, call optimize on each conditional -// optimize2(root, delta); -// return delta; -//} - -/* ************************************************************************* */ + /* ************************************************************************* */ template -int optimize2(const boost::shared_ptr& root, double threshold, const vector& keys, Permuted& delta) { - vector changed(keys.size(), false); - int count = 0; - // starting from the root, call optimize on each conditional - optimize2(root, threshold, changed, keys, delta, count); - return count; -} + void nnz_internal(const boost::shared_ptr& clique, int& result) { + int dimR = (*clique)->dim(); + int dimSep = (*clique)->get_S().cols() - dimR; + result += ((dimR+1)*dimR)/2 + dimSep*dimR; + // traverse the children + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { + nnz_internal(child, result); + } + } -/* ************************************************************************* */ + /* ************************************************************************* */ template -void nnz_internal(const boost::shared_ptr& clique, int& result) { - int dimR = (*clique)->dim(); - int dimSep = (*clique)->get_S().cols() - dimR; - result += ((dimR+1)*dimR)/2 + dimSep*dimR; - // traverse the children - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { - nnz_internal(child, result); - } -} - -/* ************************************************************************* */ - template -int calculate_nnz(const boost::shared_ptr& clique) { - int result = 0; - // starting from the root, add up entries of frontal and conditional matrices of each conditional - nnz_internal(clique, result); - return result; -} + int calculate_nnz(const boost::shared_ptr& clique) { + int result = 0; + // starting from the root, add up entries of frontal and conditional matrices of each conditional + nnz_internal(clique, result); + return result; + } } /// namespace gtsam diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2.cpp new file mode 100644 index 000000000..d56de0055 --- /dev/null +++ b/gtsam/nonlinear/GaussianISAM2.cpp @@ -0,0 +1,61 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianISAM2 + * @brief Full non-linear ISAM + * @author Michael Kaess + */ + +#include +#include + +using namespace std; +using namespace gtsam; + +#include + +namespace gtsam { + + /* ************************************************************************* */ + VectorValues gradient(const BayesTree& bayesTree, const VectorValues& x0) { + return gradient(FactorGraph(bayesTree), x0); + } + + /* ************************************************************************* */ + void gradientAtZero(const BayesTree& bayesTree, VectorValues& g) { + gradientAtZero(FactorGraph(bayesTree), g); + } + + /* ************************************************************************* */ + VectorValues gradient(const BayesTree >& bayesTree, const VectorValues& x0) { + return gradient(FactorGraph(bayesTree)); + } + + /* ************************************************************************* */ + void gradientAtZero(const BayesTree >& bayesTree, VectorValues& g) { + // Zero-out gradient + g.setZero(); + + // Sum up contributions for each clique + typedef boost::shared_ptr > sharedClique; + BOOST_FOREACH(const sharedClique& clique, bayesTree.nodes()) { + // Loop through variables in each clique, adding contributions + int variablePosition = 0; + for(GaussianConditional::const_iterator jit = clique->conditional()->beginFrontals(); jit != clique->conditional()->endFrontals(); ++jit) { + const int dim = clique->conditional()->dim(jit); + x0[*jit] += clique->gradientContribution().segment(variablePosition, dim); + variablePosition += dim; + } + } + } + +} diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index cb6f39865..f233d4c86 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 GaussianISAM * @brief Full non-linear ISAM. @@ -41,26 +52,74 @@ public: } }; -// optimize the BayesTree, starting from the root +/** optimize the BayesTree, starting from the root */ template void optimize2(const boost::shared_ptr& root, VectorValues& delta); -// optimize the BayesTree, starting from the root; "replaced" needs to contain -// all variables that are contained in the top of the Bayes tree that has been -// redone; "delta" is the current solution, an offset from the linearization -// point; "threshold" is the maximum change against the PREVIOUS delta for -// non-replaced variables that can be ignored, ie. the old delta entry is kept -// and recursive backsubstitution might eventually stop if none of the changed -// variables are contained in the subtree. -// returns the number of variables that were solved for +/// optimize the BayesTree, starting from the root; "replaced" needs to contain +/// all variables that are contained in the top of the Bayes tree that has been +/// redone; "delta" is the current solution, an offset from the linearization +/// point; "threshold" is the maximum change against the PREVIOUS delta for +/// non-replaced variables that can be ignored, ie. the old delta entry is kept +/// and recursive backsubstitution might eventually stop if none of the changed +/// variables are contained in the subtree. +/// returns the number of variables that were solved for template int optimize2(const boost::shared_ptr& root, double threshold, const std::vector& replaced, Permuted& delta); -// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) +/// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) template int calculate_nnz(const boost::shared_ptr& clique); +/** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ R^T(Rx-d) \f$. + * @param bayesTree The Gaussian Bayes Tree $(R,d)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ +VectorValues gradient(const BayesTree& bayesTree, const VectorValues& x0); + +/** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around zero. + * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). + * @param bayesTree The Gaussian Bayes Tree $(R,d)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ +VectorValues gradientAtZero(const BayesTree& bayesTree, VectorValues& g); + +/** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around \f$ x = x_0 \f$. + * The gradient is \f$ R^T(Rx-d) \f$. + * This specialized version is used with ISAM2, where each clique stores its + * gradient contribution. + * @param bayesTree The Gaussian Bayes Tree $(R,d)$ + * @param x0 The center about which to compute the gradient + * @return The gradient as a VectorValues + */ +VectorValues gradient(const BayesTree >& bayesTree, const VectorValues& x0); + +/** + * Compute the gradient of the energy function, + * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$, + * centered around zero. + * The gradient about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, const VectorValues&). + * This specialized version is used with ISAM2, where each clique stores its + * gradient contribution. + * @param bayesTree The Gaussian Bayes Tree $(R,d)$ + * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues + * @return The gradient as a VectorValues + */ +VectorValues gradientAtZero(const BayesTree >& bayesTree, VectorValues& g); + }/// namespace gtsam #include diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index b944b105d..58503466c 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -30,7 +30,8 @@ headers += DoglegOptimizer.h DoglegOptimizer-inl.h # Nonlinear iSAM(2) headers += NonlinearISAM.h NonlinearISAM-inl.h headers += ISAM2.h ISAM2-inl.h ISAM2-impl-inl.h -headers += GaussianISAM2.h GaussianISAM2-inl.h +sources += GaussianISAM2.cpp +headers += GaussianISAM2-inl.h # Nonlinear constraints headers += NonlinearEquality.h From dace9e213c598f8748068e47eeae0c84fbab5ed3 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 13 Dec 2011 22:54:11 +0000 Subject: [PATCH 14/17] (in branch) Start of unit test for incremental gradient calculation --- tests/testGaussianISAM2.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index d56d2edbc..45262e24e 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -296,6 +296,10 @@ TEST_UNSAFE(ISAM2, slamlike_solution) // Compare solutions EXPECT(isam_check(fullgraph, fullinit, isam)); + + // Check gradient + VectorValues expectedGradient(*allocateVectorValues(isam)); + gradient } /* ************************************************************************* */ From f3de9e425feeedc822d2f7ecb41e1dbf250154e2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 15 Dec 2011 00:08:57 +0000 Subject: [PATCH 15/17] (in branch) first pass at Dogleg in ISAM2, bug(s) remaining --- gtsam/inference/BayesTree.h | 2 +- gtsam/inference/BayesTreeCliqueBase.h | 3 +- gtsam/linear/HessianFactor.cpp | 4 -- gtsam/nonlinear/DoglegOptimizerImpl.cpp | 2 +- gtsam/nonlinear/DoglegOptimizerImpl.h | 2 +- gtsam/nonlinear/GaussianISAM2.cpp | 31 +++++++---- gtsam/nonlinear/GaussianISAM2.h | 4 +- gtsam/nonlinear/ISAM2-inl.h | 67 ++++++++++++++++------- gtsam/nonlinear/ISAM2.h | 71 +++++++++++++++++++++---- tests/testGaussianISAM2.cpp | 30 +++++++++-- 10 files changed, 164 insertions(+), 52 deletions(-) diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 9106949b1..650341a5c 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -36,7 +36,7 @@ namespace gtsam { // Forward declaration of BayesTreeClique which is defined below BayesTree in this file - template class BayesTreeClique; + template struct BayesTreeClique; /** * Bayes tree diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 66be72f86..75bf86449 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -39,7 +39,8 @@ namespace gtsam { * possible because all cliques in a BayesTree are the same type - if they * were not then we'd need a virtual class. * - * @tparam The derived clique type. + * @tparam DERIVED The derived clique type. + * @tparam CONDITIONAL The conditional type. */ template struct BayesTreeCliqueBase { diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 5cba4f4dc..8eb34d10c 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -20,8 +20,6 @@ #include #include #include -#include -#include #include #include @@ -38,8 +36,6 @@ using namespace std; -using namespace boost::lambda; - namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index 9316e0839..60e8d2a59 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -25,7 +25,7 @@ VectorValues DoglegOptimizerImpl::ComputeDoglegPoint( return x_d; } else if(DeltaSq < x_n_norm_sq) { // Trust region boundary is between steepest descent point and Newton's method point - return ComputeBlend(Delta, dx_u, dx_n); + return ComputeBlend(Delta, dx_u, dx_n, verbose); } else { assert(DeltaSq >= x_n_norm_sq); if(verbose) cout << "In pure Newton's method region" << endl; diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 28b4185ca..e35adaea9 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -155,7 +155,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( bool stay = true; while(stay) { // Compute dog leg point - result.dx_d = ComputeDoglegPoint(Delta, dx_u, dx_n); + result.dx_d = ComputeDoglegPoint(Delta, dx_u, dx_n, verbose); if(verbose) cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << endl; diff --git a/gtsam/nonlinear/GaussianISAM2.cpp b/gtsam/nonlinear/GaussianISAM2.cpp index d56de0055..34456ad36 100644 --- a/gtsam/nonlinear/GaussianISAM2.cpp +++ b/gtsam/nonlinear/GaussianISAM2.cpp @@ -17,6 +17,7 @@ #include #include +#include using namespace std; using namespace gtsam; @@ -37,7 +38,24 @@ namespace gtsam { /* ************************************************************************* */ VectorValues gradient(const BayesTree >& bayesTree, const VectorValues& x0) { - return gradient(FactorGraph(bayesTree)); + return gradient(FactorGraph(bayesTree), x0); + } + + /* ************************************************************************* */ + static void gradientAtZeroTreeAdder(const boost::shared_ptr >& root, VectorValues& g) { + // Loop through variables in each clique, adding contributions + int variablePosition = 0; + for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) { + const int dim = root->conditional()->dim(jit); + g[*jit] += root->gradientContribution().segment(variablePosition, dim); + variablePosition += dim; + } + + // Recursively add contributions from children + typedef boost::shared_ptr > sharedClique; + BOOST_FOREACH(const sharedClique& child, root->children()) { + gradientAtZeroTreeAdder(child, g); + } } /* ************************************************************************* */ @@ -46,16 +64,7 @@ namespace gtsam { g.setZero(); // Sum up contributions for each clique - typedef boost::shared_ptr > sharedClique; - BOOST_FOREACH(const sharedClique& clique, bayesTree.nodes()) { - // Loop through variables in each clique, adding contributions - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->beginFrontals(); jit != clique->conditional()->endFrontals(); ++jit) { - const int dim = clique->conditional()->dim(jit); - x0[*jit] += clique->gradientContribution().segment(variablePosition, dim); - variablePosition += dim; - } - } + gradientAtZeroTreeAdder(bayesTree.root(), g); } } diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index f233d4c86..4da1d3053 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -92,7 +92,7 @@ VectorValues gradient(const BayesTree& bayesTree, const Vec * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues * @return The gradient as a VectorValues */ -VectorValues gradientAtZero(const BayesTree& bayesTree, VectorValues& g); +void gradientAtZero(const BayesTree& bayesTree, VectorValues& g); /** * Compute the gradient of the energy function, @@ -118,7 +118,7 @@ VectorValues gradient(const BayesTree >& bayesTree, VectorValues& g); +void gradientAtZero(const BayesTree >& bayesTree, VectorValues& g); }/// namespace gtsam diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 5d2984c9c..8b564c093 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -28,7 +28,7 @@ using namespace boost::assign; #include #include - +#include namespace gtsam { @@ -42,12 +42,18 @@ static const bool structuralLast = false; /* ************************************************************************* */ template ISAM2::ISAM2(const ISAM2Params& params): - delta_(Permutation(), deltaUnpermuted_), params_(params) {} + delta_(Permutation(), deltaUnpermuted_), params_(params) { + if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; +} /* ************************************************************************* */ template ISAM2::ISAM2(): - delta_(Permutation(), deltaUnpermuted_) {} + delta_(Permutation(), deltaUnpermuted_) { + if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; +} /* ************************************************************************* */ template @@ -518,25 +524,37 @@ ISAM2Result ISAM2::update( tic(9,"solve"); // 9. Solve - if (params_.wildfireThreshold <= 0.0 || disableReordering) { - VectorValues newDelta(theta_.dims(ordering_)); - optimize2(this->root(), newDelta); - if(debug) newDelta.print("newDelta: "); - assert(newDelta.size() == delta_.size()); - delta_.permutation() = Permutation::Identity(delta_.size()); - delta_.container() = newDelta; - lastBacksubVariableCount = theta_.size(); - } else { - vector replacedKeysMask(variableIndex_.size(), false); - if(replacedKeys) { - BOOST_FOREACH(const Index var, *replacedKeys) { - replacedKeysMask[var] = true; } } - lastBacksubVariableCount = optimize2(this->root(), params_.wildfireThreshold, replacedKeysMask, delta_); // modifies delta_ + if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { + const ISAM2GaussNewtonParams& gaussNewtonParams = boost::get(params_.optimizationParams); + if (gaussNewtonParams.wildfireThreshold <= 0.0 || disableReordering) { + VectorValues newDelta(theta_.dims(ordering_)); + optimize2(this->root(), newDelta); + if(debug) newDelta.print("newDelta: "); + assert(newDelta.size() == delta_.size()); + delta_.permutation() = Permutation::Identity(delta_.size()); + delta_.container() = newDelta; + lastBacksubVariableCount = theta_.size(); + } else { + vector replacedKeysMask(variableIndex_.size(), false); + if(replacedKeys) { + BOOST_FOREACH(const Index var, *replacedKeys) { + replacedKeysMask[var] = true; } } + lastBacksubVariableCount = optimize2(this->root(), gaussNewtonParams.wildfireThreshold, replacedKeysMask, delta_); // modifies delta_ #ifndef NDEBUG - for(size_t j=0; j).all()); + for(size_t j=0; j).all()); #endif + } + } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { + const ISAM2DoglegParams& doglegParams = boost::get(params_.optimizationParams); + // Do one Dogleg iteration + DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate( + *doglegDelta_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose); + // 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 } toc(9,"solve"); @@ -578,5 +596,16 @@ VALUES ISAM2::calculateBestEstimate() const { return theta_.retract(delta, ordering_); } +/* ************************************************************************* */ +template +VectorValues optimize(const ISAM2& isam) { + VectorValues delta = *allocateVectorValues(isam); + for(Index j=0; j #include +#include + namespace gtsam { /** * @defgroup ISAM2 */ +/** + * @ingroup ISAM2 + * Parameters for ISAM2 using Gauss-Newton optimization. Either this class or + * ISAM2DoglegParams should be specified as the optimizationParams in + * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). + */ +struct ISAM2GaussNewtonParams { + double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 0.001) + + /** Specify parameters as constructor arguments */ + ISAM2GaussNewtonParams( + double _wildfireThreshold = 0.001 ///< see ISAM2GaussNewtonParams public variables, ISAM2GaussNewtonParams::wildfireThreshold + ) : wildfireThreshold(_wildfireThreshold) {} +}; + +/** + * @ingroup ISAM2 + * Parameters for ISAM2 using Dogleg optimization. Either this class or + * ISAM2GaussNewtonParams should be specified as the optimizationParams in + * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). + */ +struct ISAM2DoglegParams { + double initialDelta; ///< The initial trust region radius for Dogleg + bool verbose; ///< Whether Dogleg prints iteration and convergence information + + /** Specify parameters as constructor arguments */ + ISAM2DoglegParams( + double _initialDelta = 1.0, ///< see ISAM2DoglegParams public variables, ISAM2DoglegParams::initialDelta + bool _verbose = false ///< see ISAM2DoglegParams public variables, ISAM2DoglegParams::verbose + ) : initialDelta(_initialDelta), verbose(_verbose) {} +}; + /** * @ingroup ISAM2 * Parameters for the ISAM2 algorithm. Default parameter values are listed below. */ struct ISAM2Params { - double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 0.001) + typedef boost::variant OptimizationParams; ///< Either ISAM2GaussNewtonParams or ISAM2DoglegParams + /** Optimization parameters, this both selects the nonlinear optimization + * method and specifies its parameters, either ISAM2GaussNewtonParams or + * ISAM2DoglegParams. In the former, Gauss-Newton optimization will be used + * with the specified parameters, and in the latter Powell's dog-leg + * algorithm will be used with the specified parameters. + */ + OptimizationParams optimizationParams; double relinearizeThreshold; ///< Only relinearize variables whose linear delta magnitude is greater than this threshold (default: 0.1) int relinearizeSkip; ///< Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10) bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize any variables (default: true) @@ -41,12 +82,12 @@ struct ISAM2Params { /** Specify parameters as constructor arguments */ ISAM2Params( - double _wildfireThreshold = 0.001, ///< ISAM2Params::wildfireThreshold - double _relinearizeThreshold = 0.1, ///< ISAM2Params::relinearizeThreshold - int _relinearizeSkip = 10, ///< ISAM2Params::relinearizeSkip - bool _enableRelinearization = true, ///< ISAM2Params::enableRelinearization - bool _evaluateNonlinearError = false ///< ISAM2Params::evaluateNonlinearError - ) : wildfireThreshold(_wildfireThreshold), relinearizeThreshold(_relinearizeThreshold), + OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params public variables, ISAM2Params::optimizationParams + double _relinearizeThreshold = 0.1, ///< see ISAM2Params public variables, ISAM2Params::relinearizeThreshold + int _relinearizeSkip = 10, ///< see ISAM2Params public variables, ISAM2Params::relinearizeSkip + bool _enableRelinearization = true, ///< see ISAM2Params public variables, ISAM2Params::enableRelinearization + bool _evaluateNonlinearError = false ///< see ISAM2Params public variables, ISAM2Params::evaluateNonlinearError + ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold), relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), evaluateNonlinearError(_evaluateNonlinearError) {} }; @@ -153,8 +194,12 @@ struct ISAM2Clique : public BayesTreeCliqueBase, CONDIT /** print this node */ void print(const std::string& s = "") const { Base::print(s); - if(cachedFactor_) cachedFactor_->print(s + "Cached: "); - else cout << s << "Cached empty" << endl; + if(cachedFactor_) + cachedFactor_->print(s + "Cached: "); + else + cout << s << "Cached empty" << endl; + if(gradientContribution_.rows() != 0) + gtsam::print(gradientContribution_, "Gradient contribution: "); } void permuteWithInverse(const Permutation& inversePermutation) { @@ -224,6 +269,9 @@ protected: /** The current parameters */ ISAM2Params params_; + /** The current Dogleg Delta (trust region radius) */ + boost::optional doglegDelta_; + private: #ifndef NDEBUG std::vector lastRelinVariables_; @@ -258,6 +306,7 @@ public: newISAM2->nonlinearFactors_ = nonlinearFactors_; newISAM2->ordering_ = ordering_; newISAM2->params_ = params_; + newISAM2->doglegDelta_ = doglegDelta_; #ifndef NDEBUG newISAM2->lastRelinVariables_ = lastRelinVariables_; #endif @@ -350,6 +399,10 @@ private: }; // ISAM2 +/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ +template +VectorValues optimize(const ISAM2& isam); + } /// namespace gtsam #include diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 45262e24e..265d01860 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -210,7 +210,7 @@ TEST_UNSAFE(ISAM2, slamlike_solution) SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2 isam(ISAM2Params(0.001, 0.0, 0, false)); + GaussianISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); planarSLAM::Values fullinit; planarSLAM::Graph fullgraph; @@ -297,9 +297,33 @@ TEST_UNSAFE(ISAM2, slamlike_solution) // Compare solutions EXPECT(isam_check(fullgraph, fullinit, isam)); + // Check gradient at each node + typedef GaussianISAM2::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)); - gradient + 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)); } /* ************************************************************************* */ @@ -314,7 +338,7 @@ TEST_UNSAFE(ISAM2, clone) { SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2 isam(ISAM2Params(0.001, 0.0, 0, false, true)); + GaussianISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); planarSLAM::Values fullinit; planarSLAM::Graph fullgraph; From 21140ea0d586dd18e65517c6c9e6a28b2a66fb77 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 15 Dec 2011 15:37:52 +0000 Subject: [PATCH 16/17] (in branch) incremental dogleg bug fix and unit test (wasn't computing Gauss-Newton point) --- gtsam/nonlinear/ISAM2-inl.h | 5 +- tests/testGaussianISAM2.cpp | 141 +++++++++++++++++++++++++++++++++++- 2 files changed, 138 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 8b564c093..2c7d95bbe 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -600,10 +600,7 @@ VALUES ISAM2::calculateBestEstimate() const { template VectorValues optimize(const ISAM2& isam) { VectorValues delta = *allocateVectorValues(isam); - for(Index j=0; j::sharedClique sharedClique; @@ -327,7 +327,140 @@ TEST_UNSAFE(ISAM2, slamlike_solution) } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, clone) { +TEST(ISAM2, slamlike_solution_dogleg) +{ + +// SETDEBUG("ISAM2 update", true); +// SETDEBUG("ISAM2 update verbose", true); +// SETDEBUG("ISAM2 recalculate", true); + + // Pose and landmark key types from planarSLAM + typedef planarSLAM::PoseKey PoseKey; + typedef planarSLAM::PointKey PointKey; + + // Set up parameters + SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); + SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); + + // These variables will be reused and accumulate factors and values + GaussianISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); + planarSLAM::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.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert(PoseKey(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.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(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.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + fullinit.insert(PointKey(1), 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.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(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.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + fullinit.insert(PoseKey(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 GaussianISAM2::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)); +} + +/* ************************************************************************* */ +TEST(ISAM2, clone) { // Pose and landmark key types from planarSLAM typedef planarSLAM::PoseKey PoseKey; From 2fff75562f3bd60b245a8fb6f5618d4a70e4521d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 15 Dec 2011 15:53:05 +0000 Subject: [PATCH 17/17] (in branch) Merged from trunk r8058-r8097 --- .cproject | 356 +++++++++++------- CMakeLists.txt | 72 ++++ CppUnitLite/CMakeLists.txt | 9 + examples/CMakeLists.txt | 14 + examples/vSLAMexample/CMakeLists.txt | 17 + gtsam/3rdparty/CMakeLists.txt | 15 + gtsam/3rdparty/Eigen/CMakeLists.txt | 10 +- gtsam/3rdparty/Eigen/Eigen/Core | 2 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h | 4 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h | 2 +- gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h | 4 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h | 4 - .../Eigen/Eigen/src/Core/PlainObjectBase.h | 23 +- .../Eigen/Eigen/src/Core/arch/NEON/Complex.h | 4 +- .../Eigen/src/Core/arch/NEON/PacketMath.h | 56 +-- .../Core/products/GeneralBlockPanelKernel.h | 4 +- .../Eigen/Eigen/src/Core/util/Macros.h | 35 +- .../Eigen/Eigen/src/Core/util/Memory.h | 54 ++- .../Eigen/Eigen/src/Core/util/XprHelper.h | 3 +- .../src/Eigenvalues/SelfAdjointEigenSolver.h | 16 +- .../Eigen/Eigen/src/Geometry/Quaternion.h | 21 +- gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h | 10 +- .../Eigen/bench/btl/generic_bench/btl.hh | 2 +- gtsam/3rdparty/Eigen/doc/CMakeLists.txt | 5 + gtsam/3rdparty/Eigen/doc/Overview.dox | 2 +- .../TutorialLinAlgSelfAdjointEigenSolver.cpp | 3 +- gtsam/3rdparty/Eigen/test/CMakeLists.txt | 2 +- gtsam/3rdparty/Eigen/test/geo_quaternion.cpp | 5 +- gtsam/3rdparty/Eigen/test/main.h | 2 +- gtsam/3rdparty/Eigen/test/sizeoverflow.cpp | 81 ++++ .../Eigen/unsupported/Eigen/MPRealSupport | 3 +- .../Eigen/unsupported/doc/Overview.dox | 11 +- gtsam/CMakeLists.txt | 59 +++ gtsam/geometry/CalibratedCamera.cpp | 2 + gtsam/geometry/CalibratedCamera.h | 5 + gtsam/slam/ProjectionFactor.h | 15 +- tests/CMakeLists.txt | 29 ++ wrap/Argument.cpp | 12 +- wrap/CMakeLists.txt | 25 ++ 39 files changed, 754 insertions(+), 244 deletions(-) create mode 100644 CMakeLists.txt create mode 100644 CppUnitLite/CMakeLists.txt create mode 100644 examples/CMakeLists.txt create mode 100644 examples/vSLAMexample/CMakeLists.txt create mode 100644 gtsam/3rdparty/CMakeLists.txt create mode 100644 gtsam/3rdparty/Eigen/test/sizeoverflow.cpp create mode 100644 gtsam/CMakeLists.txt create mode 100644 tests/CMakeLists.txt create mode 100644 wrap/CMakeLists.txt diff --git a/.cproject b/.cproject index 857a2d913..99b0743fe 100644 --- a/.cproject +++ b/.cproject @@ -380,6 +380,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -406,7 +414,6 @@ make - tests/testBayesTree.run true false @@ -414,7 +421,6 @@ make - testBinaryBayesNet.run true false @@ -462,7 +468,6 @@ make - testSymbolicBayesNet.run true false @@ -470,7 +475,6 @@ make - tests/testSymbolicFactor.run true false @@ -478,7 +482,6 @@ make - testSymbolicFactorGraph.run true false @@ -494,20 +497,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -534,6 +528,7 @@ make + testGraph.run true false @@ -605,6 +600,7 @@ make + testInference.run true false @@ -612,6 +608,7 @@ make + testGaussianFactor.run true false @@ -619,6 +616,7 @@ make + testJunctionTree.run true false @@ -626,6 +624,7 @@ make + testSymbolicBayesNet.run true false @@ -633,6 +632,7 @@ make + testSymbolicFactorGraph.run true false @@ -702,22 +702,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -734,6 +718,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -758,15 +758,7 @@ true true - - make - -j2 - all - true - true - true - - + make -j2 check @@ -774,14 +766,6 @@ true true - - make - -j2 - clean - true - true - true - make -j2 @@ -822,7 +806,15 @@ true true - + + make + -j2 + all + true + true + true + + make -j2 check @@ -830,6 +822,14 @@ true true + + make + -j2 + clean + true + true + true + make -j2 @@ -1152,7 +1152,6 @@ make - testErrors.run true false @@ -1560,6 +1559,7 @@ make + testSimulated2DOriented.run true false @@ -1599,6 +1599,7 @@ make + testSimulated2D.run true false @@ -1606,6 +1607,7 @@ make + testSimulated3D.run true false @@ -1619,6 +1621,85 @@ true true + + cmake + .. + true + false + true + + + make + -j2 VERBOSE=1 + all + true + false + true + + + make + -j5 VERBOSE=1 + all + true + false + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + test + true + true + true + + + make + -j2 + testSimulated2D.run + true + true + true + + + make + -j2 + wrap_testWrap.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j5 + check + true + false + true + + + make + -j2 + install + true + true + true + make -j2 @@ -1853,6 +1934,7 @@ make + tests/testGaussianISAM2 true false @@ -1874,46 +1956,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - make -j2 @@ -2010,6 +2052,94 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + tests/testSpirit.run + true + true + true + + + make + -j2 + tests/testWrap.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + make -j2 @@ -2066,54 +2196,6 @@ false true - - make - -j2 - check - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - tests/testSpirit.run - true - true - true - - - make - -j2 - tests/testWrap.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 000000000..094fcd59a --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,72 @@ +project(GTSAM CXX C) +cmake_minimum_required(VERSION 2.6) + +# Set the version number for the libarary +set (GTSAM_VERSION_MAJOR 0) +set (GTSAM_VERSION_MINOR 9) +set (GTSAM_VERSION_PATCH 3) + +# guard against in-source builds +if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) + message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") +endif() + +# guard against bad build-type strings +if (NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Debug") +endif() + +string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower) +if( NOT cmake_build_type_tolower STREQUAL "debug" + AND NOT cmake_build_type_tolower STREQUAL "release" + AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo") + message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are Debug, Release, RelWithDebInfo (case-insensitive).") +endif() + +# Turn off function inlining when debugging +set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall") +set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall") +# No optimization in relwithdebinfo +set(CMAKE_C_FLAGS_RELWITHDEBINFO "-g -fno-inline -Wall") +set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -fno-inline -Wall") +# Eigen no debug in release mode +set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -DEIGEN_NO_DEBUG") +set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -DEIGEN_NO_DEBUG") + +# Configurable Options + + +# Pull in tests +enable_testing() +include(Dart) +include(CTest) + +# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck) +add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND}) + +# Find boost +find_package(Boost 1.40 REQUIRED) + +# General build settings +include_directories( + gtsam/3rdparty/UFconfig + gtsam/3rdparty/CCOLAMD/Include + ${CMAKE_SOURCE_DIR} + CppUnitLite + ${BOOST_INCLUDE_DIR}) +link_directories(${Boost_LIBRARY_DIRS}) + +# Build GTSAM library +add_subdirectory(gtsam) + +# Build CppUnitLite +add_subdirectory(CppUnitLite) + +# Build Tests +add_subdirectory(tests) + +# Build wrap +add_subdirectory(wrap) + +# Build examples +add_subdirectory(examples) \ No newline at end of file diff --git a/CppUnitLite/CMakeLists.txt b/CppUnitLite/CMakeLists.txt new file mode 100644 index 000000000..6fba22eac --- /dev/null +++ b/CppUnitLite/CMakeLists.txt @@ -0,0 +1,9 @@ +# Build/install CppUnitLite + +FILE(GLOB cppunitlite_headers "*.h") +FILE(GLOB cppunitlite_src "*.cpp") + +ADD_LIBRARY(CppUnitLite STATIC ${cppunitlite_src}) + +install(FILES ${cppunitlite_headers} DESTINATION include/CppUnitLite) +install(TARGETS CppUnitLite ARCHIVE DESTINATION lib) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 000000000..c2c4113f6 --- /dev/null +++ b/examples/CMakeLists.txt @@ -0,0 +1,14 @@ +# Build example executables +FILE(GLOB example_srcs "*.cpp") +foreach(example_src ${example_srcs} ) + get_filename_component(example_base ${example_src} NAME_WE) + set( example_bin ${example_base} ) + add_executable(${example_bin} ${example_src}) + target_link_libraries(${example_bin} gtsam) +endforeach(example_src) + +add_subdirectory(vSLAMexample) + + + + diff --git a/examples/vSLAMexample/CMakeLists.txt b/examples/vSLAMexample/CMakeLists.txt new file mode 100644 index 000000000..0297159af --- /dev/null +++ b/examples/vSLAMexample/CMakeLists.txt @@ -0,0 +1,17 @@ +# Build vSLAMexample + +set ( srcs + Feature2D.cpp + vSLAMutils.cpp +) +add_library(vSLAMexample ${srcs}) + +add_executable(vISAMexample vISAMexample.cpp) +target_link_libraries(vISAMexample vSLAMexample gtsam) + +add_executable(vSFMexample vSFMexample.cpp) +target_link_libraries(vSFMexample vSLAMexample gtsam) + + + + diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt new file mode 100644 index 000000000..ece7bace8 --- /dev/null +++ b/gtsam/3rdparty/CMakeLists.txt @@ -0,0 +1,15 @@ +# install CCOLAMD headers +install(FILES CCOLAMD/Include/ccolamd.h DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/CCOLAMD) +install(FILES UFconfig/UFconfig.h DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/UFconfig) + +# install Eigen - only the headers +install(DIRECTORY Eigen/Eigen + DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen + FILES_MATCHING PATTERN "*.h") +file(GLOB eigen_dir_headers_all "Eigen/Eigen/*") +foreach(eigen_dir ${eigen_dir_headers_all}) + get_filename_component(filename ${eigen_dir} NAME) + if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn"))) + install(FILES Eigen/Eigen/${filename} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/Eigen) + endif() +endforeach(eigen_dir) diff --git a/gtsam/3rdparty/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/CMakeLists.txt index c66a14e0e..4a6a849e9 100644 --- a/gtsam/3rdparty/Eigen/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/CMakeLists.txt @@ -283,11 +283,17 @@ if(EIGEN_BUILD_PKGCONFIG) STRING(REPLACE ${path_separator} ";" pkg_config_libdir_search "$ENV{PKG_CONFIG_LIBDIR}") message(STATUS "searching for 'pkgconfig' directory in PKG_CONFIG_LIBDIR ( $ENV{PKG_CONFIG_LIBDIR} ), ${CMAKE_INSTALL_PREFIX}/share, and ${CMAKE_INSTALL_PREFIX}/lib") FIND_PATH(pkg_config_libdir pkgconfig ${pkg_config_libdir_search} ${CMAKE_INSTALL_PREFIX}/share ${CMAKE_INSTALL_PREFIX}/lib ${pkg_config_libdir_search}) - message(STATUS "found ${pkg_config_libdir}/pkgconfig" ) + if(pkg_config_libdir) + SET(pkg_config_install_dir ${pkg_config_libdir}) + message(STATUS "found ${pkg_config_libdir}/pkgconfig" ) + else(pkg_config_libdir) + SET(pkg_config_install_dir ${CMAKE_INSTALL_PREFIX}/share) + message(STATUS "pkgconfig not found; installing in ${pkg_config_install_dir}" ) + endif(pkg_config_libdir) configure_file(eigen3.pc.in eigen3.pc) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/eigen3.pc - DESTINATION ${pkg_config_libdir}/pkgconfig + DESTINATION ${pkg_config_install_dir}/pkgconfig ) endif(EIGEN_BUILD_PKGCONFIG) diff --git a/gtsam/3rdparty/Eigen/Eigen/Core b/gtsam/3rdparty/Eigen/Eigen/Core index 6e855427c..a5025e37e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/Core +++ b/gtsam/3rdparty/Eigen/Eigen/Core @@ -167,7 +167,7 @@ #include #endif -#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(EIGEN_NO_EXCEPTIONS) +#if defined(_CPPUNWIND) || defined(__EXCEPTIONS) #define EIGEN_EXCEPTIONS #endif diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h index a3a2167ad..a11fb1b53 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h @@ -68,10 +68,8 @@ class Array friend struct internal::conservative_resize_like_impl; using Base::m_storage; + public: - enum { NeedsToAlign = (!(Options&DontAlign)) - && SizeAtCompileTime!=Dynamic && ((static_cast(sizeof(Scalar))*SizeAtCompileTime)%16)==0 }; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) using Base::base; using Base::coeff; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h index 2b251bc2c..fe3e449bf 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h @@ -94,7 +94,7 @@ struct traits MaskPacketAccessBit = (InnerSize == Dynamic || (InnerSize % packet_traits::size) == 0) && (InnerStrideAtCompileTime == 1) ? PacketAccessBit : 0, - MaskAlignedBit = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && ((OuterStrideAtCompileTime % packet_traits::size) == 0)) ? AlignedBit : 0, + MaskAlignedBit = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * sizeof(Scalar)) % 16) == 0)) ? AlignedBit : 0, FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0, FlagsLvalueBit = is_lvalue::value ? LvalueBit : 0, FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0, diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h index c23bcbfdc..9426e2d24 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h @@ -170,8 +170,8 @@ template class MapBase EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(internal::traits::Flags&PacketAccessBit, internal::inner_stride_at_compile_time::ret==1), PACKET_ACCESS_REQUIRES_TO_HAVE_INNER_STRIDE_FIXED_TO_1); - eigen_assert(EIGEN_IMPLIES(internal::traits::Flags&AlignedBit, (size_t(m_data) % (sizeof(Scalar)*internal::packet_traits::size)) == 0) - && "data is not aligned"); + eigen_assert(EIGEN_IMPLIES(internal::traits::Flags&AlignedBit, (size_t(m_data) % 16) == 0) + && "data is not aligned"); } PointerType m_data; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h index 44de22cb4..982c9256a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h @@ -153,10 +153,6 @@ class Matrix typedef typename Base::PlainObject PlainObject; - enum { NeedsToAlign = (!(Options&DontAlign)) - && SizeAtCompileTime!=Dynamic && ((static_cast(sizeof(Scalar))*SizeAtCompileTime)%16)==0 }; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) - using Base::base; using Base::coeffRef; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h index c70db9247..612254e9d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h @@ -34,6 +34,19 @@ namespace internal { +template +EIGEN_ALWAYS_INLINE void check_rows_cols_for_overflow(Index rows, Index cols) +{ + // http://hg.mozilla.org/mozilla-central/file/6c8a909977d3/xpcom/ds/CheckedInt.h#l242 + // we assume Index is signed + Index max_index = (size_t(1) << (8 * sizeof(Index) - 1)) - 1; // assume Index is signed + bool error = (rows < 0 || cols < 0) ? true + : (rows == 0 || cols == 0) ? false + : (rows > max_index / cols); + if (error) + throw_std_bad_alloc(); +} + template (Derived::IsVectorAtCompileTime)> struct conservative_resize_like_impl; template struct matrix_swap_impl; @@ -84,14 +97,12 @@ class PlainObjectBase : public internal::dense_xpr_base::type template struct StridedConstMapType { typedef Eigen::Map type; }; template struct StridedAlignedMapType { typedef Eigen::Map type; }; template struct StridedConstAlignedMapType { typedef Eigen::Map type; }; - protected: DenseStorage m_storage; public: - enum { NeedsToAlign = (!(Options&DontAlign)) - && SizeAtCompileTime!=Dynamic && ((static_cast(sizeof(Scalar))*SizeAtCompileTime)%16)==0 }; + enum { NeedsToAlign = SizeAtCompileTime != Dynamic && (internal::traits::Flags & AlignedBit) != 0 }; EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) Base& base() { return *static_cast(this); } @@ -200,11 +211,13 @@ class PlainObjectBase : public internal::dense_xpr_base::type EIGEN_STRONG_INLINE void resize(Index rows, Index cols) { #ifdef EIGEN_INITIALIZE_MATRICES_BY_ZERO + internal::check_rows_cols_for_overflow(rows, cols); Index size = rows*cols; bool size_changed = size != this->size(); m_storage.resize(size, rows, cols); if(size_changed) EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED #else + internal::check_rows_cols_for_overflow(rows, cols); m_storage.resize(rows*cols, rows, cols); #endif } @@ -273,6 +286,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type EIGEN_STRONG_INLINE void resizeLike(const EigenBase& _other) { const OtherDerived& other = _other.derived(); + internal::check_rows_cols_for_overflow(other.rows(), other.cols()); const Index othersize = other.rows()*other.cols(); if(RowsAtCompileTime == 1) { @@ -417,6 +431,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type : m_storage(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) { _check_template_params(); + internal::check_rows_cols_for_overflow(other.derived().rows(), other.derived().cols()); Base::operator=(other.derived()); } @@ -581,6 +596,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type { eigen_assert(rows >= 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows) && cols >= 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols)); + internal::check_rows_cols_for_overflow(rows, cols); m_storage.resize(rows*cols,rows,cols); EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED } @@ -638,6 +654,7 @@ struct internal::conservative_resize_like_impl if ( ( Derived::IsRowMajor && _this.cols() == cols) || // row-major and we change only the number of rows (!Derived::IsRowMajor && _this.rows() == rows) ) // column-major and we change only the number of columns { + internal::check_rows_cols_for_overflow(rows, cols); _this.derived().m_storage.conservativeResize(rows*cols,rows,cols); } else diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h index 8e55548c9..212887184 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h @@ -27,8 +27,8 @@ namespace internal { -static uint32x4_t p4ui_CONJ_XOR = { 0x00000000, 0x80000000, 0x00000000, 0x80000000 }; -static uint32x2_t p2ui_CONJ_XOR = { 0x00000000, 0x80000000 }; +static uint32x4_t p4ui_CONJ_XOR = EIGEN_INIT_NEON_PACKET4(0x00000000, 0x80000000, 0x00000000, 0x80000000); +static uint32x2_t p2ui_CONJ_XOR = EIGEN_INIT_NEON_PACKET2(0x00000000, 0x80000000); //---------- float ---------- struct Packet2cf diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h index 478ef8038..6c7cd1590 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h @@ -52,6 +52,16 @@ typedef uint32x4_t Packet4ui; #define _EIGEN_DECLARE_CONST_Packet4i(NAME,X) \ const Packet4i p4i_##NAME = pset1(X) +#if defined(__llvm__) && !defined(__clang__) + //Special treatment for Apple's llvm-gcc, its NEON packet types are unions + #define EIGEN_INIT_NEON_PACKET2(X, Y) {{X, Y}} + #define EIGEN_INIT_NEON_PACKET4(X, Y, Z, W) {{X, Y, Z, W}} +#else + //Default initializer for packets + #define EIGEN_INIT_NEON_PACKET2(X, Y) {X, Y} + #define EIGEN_INIT_NEON_PACKET4(X, Y, Z, W) {X, Y, Z, W} +#endif + #ifndef __pld #define __pld(x) asm volatile ( " pld [%[addr]]\n" :: [addr] "r" (x) : "cc" ); #endif @@ -84,7 +94,7 @@ template<> struct packet_traits : default_packet_traits }; }; -#if EIGEN_GNUC_AT_MOST(4,4) +#if EIGEN_GNUC_AT_MOST(4,4) && !defined(__llvm__) // workaround gcc 4.2, 4.3 and 4.4 compilatin issue EIGEN_STRONG_INLINE float32x4_t vld1q_f32(const float* x) { return ::vld1q_f32((const float32_t*)x); } EIGEN_STRONG_INLINE float32x2_t vld1_f32 (const float* x) { return ::vld1_f32 ((const float32_t*)x); } @@ -100,12 +110,12 @@ template<> EIGEN_STRONG_INLINE Packet4i pset1(const int& from) { template<> EIGEN_STRONG_INLINE Packet4f plset(const float& a) { - Packet4f countdown = { 0, 1, 2, 3 }; + Packet4f countdown = EIGEN_INIT_NEON_PACKET4(0, 1, 2, 3); return vaddq_f32(pset1(a), countdown); } template<> EIGEN_STRONG_INLINE Packet4i plset(const int& a) { - Packet4i countdown = { 0, 1, 2, 3 }; + Packet4i countdown = EIGEN_INIT_NEON_PACKET4(0, 1, 2, 3); return vaddq_s32(pset1(a), countdown); } @@ -395,25 +405,29 @@ template<> EIGEN_STRONG_INLINE int predux_max(const Packet4i& a) return s[0]; } -template -struct palign_impl -{ - EIGEN_STRONG_INLINE static void run(Packet4f& first, const Packet4f& second) - { - if (Offset!=0) - first = vextq_f32(first, second, Offset); - } -}; +// this PALIGN_NEON business is to work around a bug in LLVM Clang 3.0 causing incorrect compilation errors, +// see bug 347 and this LLVM bug: http://llvm.org/bugs/show_bug.cgi?id=11074 +#define PALIGN_NEON(Offset,Type,Command) \ +template<>\ +struct palign_impl\ +{\ + EIGEN_STRONG_INLINE static void run(Type& first, const Type& second)\ + {\ + if (Offset!=0)\ + first = Command(first, second, Offset);\ + }\ +};\ -template -struct palign_impl -{ - EIGEN_STRONG_INLINE static void run(Packet4i& first, const Packet4i& second) - { - if (Offset!=0) - first = vextq_s32(first, second, Offset); - } -}; +PALIGN_NEON(0,Packet4f,vextq_f32) +PALIGN_NEON(1,Packet4f,vextq_f32) +PALIGN_NEON(2,Packet4f,vextq_f32) +PALIGN_NEON(3,Packet4f,vextq_f32) +PALIGN_NEON(0,Packet4i,vextq_s32) +PALIGN_NEON(1,Packet4i,vextq_s32) +PALIGN_NEON(2,Packet4i,vextq_s32) +PALIGN_NEON(3,Packet4i,vextq_s32) + +#undef PALIGN_NEON } // end namespace internal diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h index 6f3f27170..c601f4771 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h @@ -118,14 +118,14 @@ inline void computeProductBlockingSizes(std::ptrdiff_t& k, std::ptrdiff_t& m, st // FIXME (a bit overkill maybe ?) template struct gebp_madd_selector { - EIGEN_STRONG_INLINE EIGEN_ALWAYS_INLINE_ATTRIB static void run(const CJ& cj, A& a, B& b, C& c, T& /*t*/) + EIGEN_ALWAYS_INLINE static void run(const CJ& cj, A& a, B& b, C& c, T& /*t*/) { c = cj.pmadd(a,b,c); } }; template struct gebp_madd_selector { - EIGEN_STRONG_INLINE EIGEN_ALWAYS_INLINE_ATTRIB static void run(const CJ& cj, T& a, T& b, T& c, T& t) + EIGEN_ALWAYS_INLINE static void run(const CJ& cj, T& a, T& b, T& c, T& t) { t = b; t = cj.pmul(a,t); c = padd(c,t); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h index 2a647cfda..e6c5ef425 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h @@ -28,7 +28,7 @@ #define EIGEN_WORLD_VERSION 3 #define EIGEN_MAJOR_VERSION 0 -#define EIGEN_MINOR_VERSION 3 +#define EIGEN_MINOR_VERSION 4 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ @@ -45,7 +45,7 @@ #define EIGEN_GNUC_AT_MOST(x,y) 0 #endif -#if EIGEN_GNUC_AT_MOST(4,3) +#if EIGEN_GNUC_AT_MOST(4,3) && !defined(__clang__) // see bug 89 #define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 0 #else @@ -130,31 +130,34 @@ #define EIGEN_MAKESTRING2(a) #a #define EIGEN_MAKESTRING(a) EIGEN_MAKESTRING2(a) -// EIGEN_ALWAYS_INLINE_ATTRIB should be use in the declaration of function -// which should be inlined even in debug mode. -// FIXME with the always_inline attribute, -// gcc 3.4.x reports the following compilation error: -// Eval.h:91: sorry, unimplemented: inlining failed in call to 'const Eigen::Eval Eigen::MatrixBase::eval() const' -// : function body not available -#if EIGEN_GNUC_AT_LEAST(4,0) -#define EIGEN_ALWAYS_INLINE_ATTRIB __attribute__((always_inline)) -#else -#define EIGEN_ALWAYS_INLINE_ATTRIB -#endif - #if EIGEN_GNUC_AT_LEAST(4,1) && !defined(__clang__) && !defined(__INTEL_COMPILER) #define EIGEN_FLATTEN_ATTRIB __attribute__((flatten)) #else #define EIGEN_FLATTEN_ATTRIB #endif -// EIGEN_FORCE_INLINE means "inline as much as possible" +// EIGEN_STRONG_INLINE is a stronger version of the inline, using __forceinline on MSVC, +// but it still doesn't use GCC's always_inline. This is useful in (common) situations where MSVC needs forceinline +// but GCC is still doing fine with just inline. #if (defined _MSC_VER) || (defined __INTEL_COMPILER) #define EIGEN_STRONG_INLINE __forceinline #else #define EIGEN_STRONG_INLINE inline #endif +// EIGEN_ALWAYS_INLINE is the stronget, it has the effect of making the function inline and adding every possible +// attribute to maximize inlining. This should only be used when really necessary: in particular, +// it uses __attribute__((always_inline)) on GCC, which most of the time is useless and can severely harm compile times. +// FIXME with the always_inline attribute, +// gcc 3.4.x reports the following compilation error: +// Eval.h:91: sorry, unimplemented: inlining failed in call to 'const Eigen::Eval Eigen::MatrixBase::eval() const' +// : function body not available +#if EIGEN_GNUC_AT_LEAST(4,0) +#define EIGEN_ALWAYS_INLINE __attribute__((always_inline)) inline +#else +#define EIGEN_ALWAYS_INLINE EIGEN_STRONG_INLINE +#endif + #if (defined __GNUC__) #define EIGEN_DONT_INLINE __attribute__((noinline)) #elif (defined _MSC_VER) @@ -249,7 +252,7 @@ #define EIGEN_UNUSED_VARIABLE(var) (void)var; #if (defined __GNUC__) -#define EIGEN_ASM_COMMENT(X) asm("#"X) +#define EIGEN_ASM_COMMENT(X) asm("#" X) #else #define EIGEN_ASM_COMMENT(X) #endif diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h index a580b95ad..023716dc9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h @@ -82,6 +82,16 @@ namespace internal { +inline void throw_std_bad_alloc() +{ + #ifdef EIGEN_EXCEPTIONS + throw std::bad_alloc(); + #else + std::size_t huge = -1; + new int[huge]; + #endif +} + /***************************************************************************** *** Implementation of handmade aligned functions *** *****************************************************************************/ @@ -192,7 +202,7 @@ inline void check_that_malloc_is_allowed() #endif /** \internal Allocates \a size bytes. The returned pointer is guaranteed to have 16 bytes alignment. - * On allocation error, the returned pointer is null, and if exceptions are enabled then a std::bad_alloc is thrown. + * On allocation error, the returned pointer is null, and std::bad_alloc is thrown. */ inline void* aligned_malloc(size_t size) { @@ -213,10 +223,9 @@ inline void* aligned_malloc(size_t size) result = handmade_aligned_malloc(size); #endif - #ifdef EIGEN_EXCEPTIONS - if(result == 0) - throw std::bad_alloc(); - #endif + if(!result && size) + throw_std_bad_alloc(); + return result; } @@ -241,7 +250,7 @@ inline void aligned_free(void *ptr) /** * \internal * \brief Reallocates an aligned block of memory. -* \throws std::bad_alloc if EIGEN_EXCEPTIONS are defined. +* \throws std::bad_alloc on allocation failure **/ inline void* aligned_realloc(void *ptr, size_t new_size, size_t old_size) { @@ -269,10 +278,9 @@ inline void* aligned_realloc(void *ptr, size_t new_size, size_t old_size) result = handmade_aligned_realloc(ptr,new_size,old_size); #endif -#ifdef EIGEN_EXCEPTIONS - if (result==0 && new_size!=0) - throw std::bad_alloc(); -#endif + if (!result && new_size) + throw_std_bad_alloc(); + return result; } @@ -281,7 +289,7 @@ inline void* aligned_realloc(void *ptr, size_t new_size, size_t old_size) *****************************************************************************/ /** \internal Allocates \a size bytes. If Align is true, then the returned ptr is 16-byte-aligned. - * On allocation error, the returned pointer is null, and if exceptions are enabled then a std::bad_alloc is thrown. + * On allocation error, the returned pointer is null, and a std::bad_alloc is thrown. */ template inline void* conditional_aligned_malloc(size_t size) { @@ -293,9 +301,8 @@ template<> inline void* conditional_aligned_malloc(size_t size) check_that_malloc_is_allowed(); void *result = std::malloc(size); - #ifdef EIGEN_EXCEPTIONS - if(!result) throw std::bad_alloc(); - #endif + if(!result && size) + throw_std_bad_alloc(); return result; } @@ -347,18 +354,27 @@ template inline void destruct_elements_of_array(T *ptr, size_t size) *** Implementation of aligned new/delete-like functions *** *****************************************************************************/ +template +EIGEN_ALWAYS_INLINE void check_size_for_overflow(size_t size) +{ + if(size > size_t(-1) / sizeof(T)) + throw_std_bad_alloc(); +} + /** \internal Allocates \a size objects of type T. The returned pointer is guaranteed to have 16 bytes alignment. - * On allocation error, the returned pointer is undefined, but if exceptions are enabled then a std::bad_alloc is thrown. + * On allocation error, the returned pointer is undefined, but a std::bad_alloc is thrown. * The default constructor of T is called. */ template inline T* aligned_new(size_t size) { + check_size_for_overflow(size); T *result = reinterpret_cast(aligned_malloc(sizeof(T)*size)); return construct_elements_of_array(result, size); } template inline T* conditional_aligned_new(size_t size) { + check_size_for_overflow(size); T *result = reinterpret_cast(conditional_aligned_malloc(sizeof(T)*size)); return construct_elements_of_array(result, size); } @@ -383,6 +399,8 @@ template inline void conditional_aligned_delete(T *ptr, template inline T* conditional_aligned_realloc_new(T* pts, size_t new_size, size_t old_size) { + check_size_for_overflow(new_size); + check_size_for_overflow(old_size); if(new_size < old_size) destruct_elements_of_array(pts+new_size, old_size-new_size); T *result = reinterpret_cast(conditional_aligned_realloc(reinterpret_cast(pts), sizeof(T)*new_size, sizeof(T)*old_size)); @@ -394,6 +412,7 @@ template inline T* conditional_aligned_realloc_new(T* pt template inline T* conditional_aligned_new_auto(size_t size) { + check_size_for_overflow(size); T *result = reinterpret_cast(conditional_aligned_malloc(sizeof(T)*size)); if(NumTraits::RequireInitialization) construct_elements_of_array(result, size); @@ -402,6 +421,8 @@ template inline T* conditional_aligned_new_auto(size_t s template inline T* conditional_aligned_realloc_new_auto(T* pts, size_t new_size, size_t old_size) { + check_size_for_overflow(new_size); + check_size_for_overflow(old_size); if(NumTraits::RequireInitialization && (new_size < old_size)) destruct_elements_of_array(pts+new_size, old_size-new_size); T *result = reinterpret_cast(conditional_aligned_realloc(reinterpret_cast(pts), sizeof(T)*new_size, sizeof(T)*old_size)); @@ -536,6 +557,7 @@ template class aligned_stack_memory_handler #endif #define ei_declare_aligned_stack_constructed_variable(TYPE,NAME,SIZE,BUFFER) \ + Eigen::internal::check_size_for_overflow(SIZE); \ TYPE* NAME = (BUFFER)!=0 ? (BUFFER) \ : reinterpret_cast( \ (sizeof(TYPE)*SIZE<=EIGEN_STACK_ALLOCATION_LIMIT) ? EIGEN_ALIGNED_ALLOCA(sizeof(TYPE)*SIZE) \ @@ -545,6 +567,7 @@ template class aligned_stack_memory_handler #else #define ei_declare_aligned_stack_constructed_variable(TYPE,NAME,SIZE,BUFFER) \ + Eigen::internal::check_size_for_overflow(SIZE); \ TYPE* NAME = (BUFFER)!=0 ? BUFFER : reinterpret_cast(Eigen::internal::aligned_malloc(sizeof(TYPE)*SIZE)); \ Eigen::internal::aligned_stack_memory_handler EIGEN_CAT(NAME,_stack_memory_destructor)((BUFFER)==0 ? NAME : 0,SIZE,true) @@ -669,6 +692,7 @@ public: pointer allocate( size_type num, const void* hint = 0 ) { EIGEN_UNUSED_VARIABLE(hint); + internal::check_size_for_overflow(num); return static_cast( internal::aligned_malloc( num * sizeof(T) ) ); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h index 9047c5f83..fcd3b093f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h @@ -125,10 +125,9 @@ class compute_matrix_flags aligned_bit = ( ((Options&DontAlign)==0) - && packet_traits::Vectorizable && ( #if EIGEN_ALIGN_STATICALLY - ((!is_dynamic_size_storage) && (((MaxCols*MaxRows) % packet_traits::size) == 0)) + ((!is_dynamic_size_storage) && (((MaxCols*MaxRows*sizeof(Scalar)) % 16) == 0)) #else 0 #endif diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h index 965dda88b..c8945a848 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h @@ -220,6 +220,7 @@ template class SelfAdjointEigenSolver const MatrixType& eigenvectors() const { eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); + eigen_assert(info() == Success && "Eigenvalue computation did not converge."); eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); return m_eivec; } @@ -242,6 +243,7 @@ template class SelfAdjointEigenSolver const RealVectorType& eigenvalues() const { eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); + eigen_assert(info() == Success && "Eigenvalue computation did not converge."); return m_eivalues; } @@ -266,6 +268,7 @@ template class SelfAdjointEigenSolver MatrixType operatorSqrt() const { eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); + eigen_assert(info() == Success && "Eigenvalue computation did not converge."); eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); return m_eivec * m_eivalues.cwiseSqrt().asDiagonal() * m_eivec.adjoint(); } @@ -291,6 +294,7 @@ template class SelfAdjointEigenSolver MatrixType operatorInverseSqrt() const { eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized."); + eigen_assert(info() == Success && "Eigenvalue computation did not converge."); eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues."); return m_eivec * m_eivalues.cwiseInverse().cwiseSqrt().asDiagonal() * m_eivec.adjoint(); } @@ -307,7 +311,8 @@ template class SelfAdjointEigenSolver /** \brief Maximum number of iterations. * - * Maximum number of iterations allowed for an eigenvalue to converge. + * The algorithm terminates if it does not converge within m_maxIterations * n iterations, where n + * denotes the size of the matrix. This value is currently set to 30 (copied from LAPACK). */ static const int m_maxIterations = 30; @@ -407,7 +412,7 @@ SelfAdjointEigenSolver& SelfAdjointEigenSolver Index end = n-1; Index start = 0; - Index iter = 0; // number of iterations we are working on one element + Index iter = 0; // total number of iterations while (end>0) { @@ -418,15 +423,14 @@ SelfAdjointEigenSolver& SelfAdjointEigenSolver // find the largest unreduced block while (end>0 && m_subdiag[end-1]==0) { - iter = 0; end--; } if (end<=0) break; - // if we spent too many iterations on the current element, we give up + // if we spent too many iterations, we give up iter++; - if(iter > m_maxIterations) break; + if(iter > m_maxIterations * n) break; start = end - 1; while (start>0 && m_subdiag[start-1]!=0) @@ -435,7 +439,7 @@ SelfAdjointEigenSolver& SelfAdjointEigenSolver internal::tridiagonal_qr_step(diag.data(), m_subdiag.data(), start, end, computeEigenvectors ? m_eivec.data() : (Scalar*)0, n); } - if (iter <= m_maxIterations) + if (iter <= m_maxIterations * n) m_info = Success; else m_info = NoConvergence; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h index 2662d60fe..b2f127b6a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h @@ -182,10 +182,9 @@ public: template inline typename internal::cast_return_type >::type cast() const { - return typename internal::cast_return_type >::type( - coeffs().template cast()); + return typename internal::cast_return_type >::type(derived()); } - + #ifdef EIGEN_QUATERNIONBASE_PLUGIN # include EIGEN_QUATERNIONBASE_PLUGIN #endif @@ -225,22 +224,25 @@ struct traits > typedef _Scalar Scalar; typedef Matrix<_Scalar,4,1,_Options> Coefficients; enum{ - IsAligned = bool(EIGEN_ALIGN) && ((int(_Options)&Aligned)==Aligned), + IsAligned = internal::traits::Flags & AlignedBit, Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit }; }; } template -class Quaternion : public QuaternionBase >{ +class Quaternion : public QuaternionBase > +{ typedef QuaternionBase > Base; + enum { IsAligned = internal::traits::IsAligned }; + public: typedef _Scalar Scalar; EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion) using Base::operator*=; - typedef typename internal::traits >::Coefficients Coefficients; + typedef typename internal::traits::Coefficients Coefficients; typedef typename Base::AngleAxisType AngleAxisType; /** Default constructor leaving the quaternion uninitialized. */ @@ -271,9 +273,16 @@ public: template explicit inline Quaternion(const MatrixBase& other) { *this = other; } + /** Explicit copy constructor with scalar conversion */ + template + explicit inline Quaternion(const Quaternion& other) + { m_coeffs = other.coeffs().template cast(); } + inline Coefficients& coeffs() { return m_coeffs;} inline const Coefficients& coeffs() const { return m_coeffs;} + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned) + protected: Coefficients m_coeffs; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h index 633fb23fd..46ae7d651 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h @@ -443,7 +443,6 @@ FullPivLU& FullPivLU::compute(const MatrixType& matrix) m_nonzero_pivots = size; // the generic case is that in which all pivots are nonzero (invertible case) m_maxpivot = RealScalar(0); - RealScalar cutoff(0); for(Index k = 0; k < size; ++k) { @@ -458,14 +457,7 @@ FullPivLU& FullPivLU::compute(const MatrixType& matrix) row_of_biggest_in_corner += k; // correct the values! since they were computed in the corner, col_of_biggest_in_corner += k; // need to add k to them. - // when k==0, biggest_in_corner is the biggest coeff absolute value in the original matrix - if(k == 0) cutoff = biggest_in_corner * NumTraits::epsilon(); - - // if the pivot (hence the corner) is "zero", terminate to avoid generating nan/inf values. - // Notice that using an exact comparison (biggest_in_corner==0) here, as Golub-van Loan do in - // their pseudo-code, results in numerical instability! The cutoff here has been validated - // by running the unit test 'lu' with many repetitions. - if(biggest_in_corner < cutoff) + if(biggest_in_corner==RealScalar(0)) { // before exiting, make sure to initialize the still uninitialized transpositions // in a sane state without destroying what we already have. diff --git a/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh b/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh index 17cd397a1..f1a88ff74 100644 --- a/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh +++ b/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh @@ -39,7 +39,7 @@ #endif #if (defined __GNUC__) -#define BTL_ASM_COMMENT(X) asm("#"X) +#define BTL_ASM_COMMENT(X) asm("#" X) #else #define BTL_ASM_COMMENT(X) #endif diff --git a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt index 43f31b91e..50ce7ee0c 100644 --- a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt @@ -64,9 +64,14 @@ add_custom_target( add_dependencies(doc-eigen-prerequisites all_snippets all_examples) add_dependencies(doc-unsupported-prerequisites unsupported_snippets unsupported_examples) + add_custom_target(doc ALL COMMAND doxygen Doxyfile-unsupported COMMAND doxygen COMMAND doxygen Doxyfile-unsupported # run doxygen twice to get proper eigen <=> unsupported cross references + COMMAND ${CMAKE_COMMAND} -E rename html eigen-doc + COMMAND ${CMAKE_COMMAND} -E tar cvfz eigen-doc/eigen-doc.tgz eigen-doc/*.html eigen-doc/*.map eigen-doc/*.png eigen-doc/*.css eigen-doc/*.js eigen-doc/*.txt eigen-doc/unsupported + COMMAND ${CMAKE_COMMAND} -E rename eigen-doc html WORKING_DIRECTORY ${Eigen_BINARY_DIR}/doc) + add_dependencies(doc doc-eigen-prerequisites doc-unsupported-prerequisites) diff --git a/gtsam/3rdparty/Eigen/doc/Overview.dox b/gtsam/3rdparty/Eigen/doc/Overview.dox index 04bc075f0..598a96b4d 100644 --- a/gtsam/3rdparty/Eigen/doc/Overview.dox +++ b/gtsam/3rdparty/Eigen/doc/Overview.dox @@ -8,7 +8,7 @@ o /** \mainpage Eigen | \ref QuickRefPage "Short reference" -This is the API documentation for Eigen3. +This is the API documentation for Eigen3. You can download it as a tgz archive for offline reading. Eigen2 users: here is a \ref Eigen2ToEigen3 guide to help porting your application. diff --git a/gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSelfAdjointEigenSolver.cpp b/gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSelfAdjointEigenSolver.cpp index e98444347..8d1d1ed65 100644 --- a/gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSelfAdjointEigenSolver.cpp +++ b/gtsam/3rdparty/Eigen/doc/examples/TutorialLinAlgSelfAdjointEigenSolver.cpp @@ -10,8 +10,9 @@ int main() A << 1, 2, 2, 3; cout << "Here is the matrix A:\n" << A << endl; SelfAdjointEigenSolver eigensolver(A); + if (eigensolver.info() != Success) abort(); cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl; - cout << "Here's a matrix whose columns are eigenvectors of A " + cout << "Here's a matrix whose columns are eigenvectors of A \n" << "corresponding to these eigenvalues:\n" << eigensolver.eigenvectors() << endl; } diff --git a/gtsam/3rdparty/Eigen/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/CMakeLists.txt index adaecb5f8..fab7de0d4 100644 --- a/gtsam/3rdparty/Eigen/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/CMakeLists.txt @@ -121,7 +121,7 @@ ei_add_test(nullary) ei_add_test(nesting_ops "${CMAKE_CXX_FLAGS_DEBUG}") ei_add_test(zerosized) ei_add_test(dontalign) - +ei_add_test(sizeoverflow) ei_add_test(prec_inverse_4x4) string(TOLOWER "${CMAKE_CXX_COMPILER}" cmake_cxx_compiler_tolower) diff --git a/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp b/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp index e03245654..1e7b2cba0 100644 --- a/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_quaternion.cpp @@ -120,6 +120,10 @@ template void quaternion(void) VERIFY_IS_APPROX(q1f.template cast(),q1); Quaternion q1d = q1.template cast(); VERIFY_IS_APPROX(q1d.template cast(),q1); + + // test bug 369 - improper alignment. + Quaternionx *q = new Quaternionx; + delete q; } template void mapQuaternion(void){ @@ -191,7 +195,6 @@ template void check_const_correctness(const PlainObjec VERIFY( !(Map::Flags & LvalueBit) ); } - void test_geo_quaternion() { for(int i = 0; i < g_repeat; i++) { diff --git a/gtsam/3rdparty/Eigen/test/main.h b/gtsam/3rdparty/Eigen/test/main.h index 4ddd11e6b..4510c1905 100644 --- a/gtsam/3rdparty/Eigen/test/main.h +++ b/gtsam/3rdparty/Eigen/test/main.h @@ -118,7 +118,7 @@ namespace Eigen } \ else if (Eigen::internal::push_assert) \ { \ - eigen_assert_list.push_back(std::string(EI_PP_MAKE_STRING(__FILE__)" ("EI_PP_MAKE_STRING(__LINE__)") : "#a) ); \ + eigen_assert_list.push_back(std::string(EI_PP_MAKE_STRING(__FILE__) " (" EI_PP_MAKE_STRING(__LINE__) ") : " #a) ); \ } #define VERIFY_RAISES_ASSERT(a) \ diff --git a/gtsam/3rdparty/Eigen/test/sizeoverflow.cpp b/gtsam/3rdparty/Eigen/test/sizeoverflow.cpp new file mode 100644 index 000000000..78ededbea --- /dev/null +++ b/gtsam/3rdparty/Eigen/test/sizeoverflow.cpp @@ -0,0 +1,81 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2011 Benoit Jacob +// +// Eigen is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 3 of the License, or (at your option) any later version. +// +// Alternatively, you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of +// the License, or (at your option) any later version. +// +// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public +// License and a copy of the GNU General Public License along with +// Eigen. If not, see . + +#include "main.h" + +#define VERIFY_THROWS_BADALLOC(a) { \ + bool threw = false; \ + try { \ + a; \ + } \ + catch (std::bad_alloc&) { threw = true; } \ + VERIFY(threw && "should have thrown bad_alloc: " #a); \ + } + +typedef DenseIndex Index; + +template +void triggerMatrixBadAlloc(Index rows, Index cols) +{ + VERIFY_THROWS_BADALLOC( MatrixType m(rows, cols) ); + VERIFY_THROWS_BADALLOC( MatrixType m; m.resize(rows, cols) ); + VERIFY_THROWS_BADALLOC( MatrixType m; m.conservativeResize(rows, cols) ); +} + +template +void triggerVectorBadAlloc(Index size) +{ + VERIFY_THROWS_BADALLOC( VectorType v(size) ); + VERIFY_THROWS_BADALLOC( VectorType v; v.resize(size) ); + VERIFY_THROWS_BADALLOC( VectorType v; v.conservativeResize(size) ); +} + +void test_sizeoverflow() +{ + // there are 2 levels of overflow checking. first in PlainObjectBase.h we check for overflow in rows*cols computations. + // this is tested in tests of the form times_itself_gives_0 * times_itself_gives_0 + // Then in Memory.h we check for overflow in size * sizeof(T) computations. + // this is tested in tests of the form times_4_gives_0 * sizeof(float) + + size_t times_itself_gives_0 = size_t(1) << (8 * sizeof(Index) / 2); + VERIFY(times_itself_gives_0 * times_itself_gives_0 == 0); + + size_t times_4_gives_0 = size_t(1) << (8 * sizeof(Index) - 2); + VERIFY(times_4_gives_0 * 4 == 0); + + size_t times_8_gives_0 = size_t(1) << (8 * sizeof(Index) - 3); + VERIFY(times_8_gives_0 * 8 == 0); + + triggerMatrixBadAlloc(times_itself_gives_0, times_itself_gives_0); + triggerMatrixBadAlloc(times_itself_gives_0 / 4, times_itself_gives_0); + triggerMatrixBadAlloc(times_4_gives_0, 1); + + triggerMatrixBadAlloc(times_itself_gives_0, times_itself_gives_0); + triggerMatrixBadAlloc(times_itself_gives_0 / 8, times_itself_gives_0); + triggerMatrixBadAlloc(times_8_gives_0, 1); + + triggerVectorBadAlloc(times_4_gives_0); + + triggerVectorBadAlloc(times_8_gives_0); +} diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/MPRealSupport b/gtsam/3rdparty/Eigen/unsupported/Eigen/MPRealSupport index 10fa23b35..8f2396353 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/MPRealSupport +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/MPRealSupport @@ -35,7 +35,8 @@ namespace Eigen { - /** \defgroup MPRealSupport_Module MPFRC++ Support module + /** \ingroup Unsupported_modules + * \defgroup MPRealSupport_Module MPFRC++ Support module * * \code * #include diff --git a/gtsam/3rdparty/Eigen/unsupported/doc/Overview.dox b/gtsam/3rdparty/Eigen/unsupported/doc/Overview.dox index c09f06857..458b507b5 100644 --- a/gtsam/3rdparty/Eigen/unsupported/doc/Overview.dox +++ b/gtsam/3rdparty/Eigen/unsupported/doc/Overview.dox @@ -1,13 +1,22 @@ namespace Eigen { -o /** \mainpage Eigen's unsupported modules +/** \mainpage Eigen's unsupported modules This is the API documentation for Eigen's unsupported modules. These modules are contributions from various users. They are provided "as is", without any support. +Click on the \e Modules tab at the top of this page to get a list of all unsupported modules. + Don't miss the official Eigen documentation. + +\defgroup Unsupported_modules Unsupported modules + +The unsupported modules are contributions from various users. They are +provided "as is", without any support. Nevertheless, some of them are +subject to be included in Eigen in the future. + */ } diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt new file mode 100644 index 000000000..5b795b11f --- /dev/null +++ b/gtsam/CMakeLists.txt @@ -0,0 +1,59 @@ +# Build full gtsam library as a single library +# and also build tests +set (gtsam_subdirs + base + geometry + inference + linear + nonlinear + slam) + +set (ccolamd_srcs + 3rdparty/CCOLAMD/Source/ccolamd.c + 3rdparty/CCOLAMD/Source/ccolamd_global.c + 3rdparty/UFconfig/UFconfig.c) + +# install headers from 3rdparty libraries +add_subdirectory(3rdparty) + +# Accumulate gtsam_srcs +set(gtsam_srcs ${ccolamd_srcs}) + +# Get all sources and headers from each +foreach(subdir ${gtsam_subdirs}) + message(STATUS "Building ${subdir}") + file(GLOB sub_gtsam_srcs "${subdir}/*.cpp") + list(APPEND gtsam_srcs ${sub_gtsam_srcs}) + + # install headers + file(GLOB sub_gtsam_headers "${subdir}/*.h") + install(FILES ${sub_gtsam_headers} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/gtsam/${subdir}) + + # Build tests + file(GLOB tests_srcs "${subdir}/tests/test*.cpp") + foreach(test_src ${tests_srcs}) + get_filename_component(test_base ${test_src} NAME_WE) + set( test_bin ${subdir}_${test_base} ) + add_executable(${test_bin} EXCLUDE_FROM_ALL ${test_src}) + add_dependencies(check ${test_bin}) + add_test(${subdir}/${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin}) + target_link_libraries(${test_bin} CppUnitLite gtsam) + add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN}) + endforeach(test_src) + + # Build timing scripts + file(GLOB time_srcs "${subdir}/tests/time*.cpp") + foreach(time_src ${time_srcs}) + get_filename_component(time_base ${time_src} NAME_WE) + set( time_bin ${time_base} ) + add_executable(${time_bin} EXCLUDE_FROM_ALL ${time_src}) + add_dependencies(check ${time_bin}) + target_link_libraries(${time_bin} CppUnitLite gtsam) + add_custom_target(${time_base}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN}) + endforeach(time_src) +endforeach(subdir) + +# build a single shared library +add_library(gtsam SHARED ${gtsam_srcs}) +install(TARGETS gtsam LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib) + diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index f0c70408e..a45da9789 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -61,6 +61,8 @@ Point2 CalibratedCamera::project(const Point3& point, const Rot3& R = pose.rotation(); const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); Point3 q = pose.transform_to(point); + if(q.z() <= 0) + throw CheiralityException(); if (D_intrinsic_pose || D_intrinsic_point) { double X = q.x(), Y = q.y(), Z = q.z(); diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 880bb9316..4538b3367 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -23,6 +23,11 @@ namespace gtsam { + class CheiralityException: public std::runtime_error { + public: + CheiralityException() : std::runtime_error("Cheirality Exception") {} + }; + /** * A Calibrated camera class [R|-R't], calibration K=I. * If calibration is known, it is more computationally efficient diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index ee5649f78..6940e367f 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -82,9 +82,18 @@ namespace gtsam { /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const Pose3& pose, const Point3& point, boost::optional H1, boost::optional H2) const { - SimpleCamera camera(*K_, pose); - Point2 reprojectionError(camera.project(point, H1, H2) - z_); - return reprojectionError.vector(); + try { + SimpleCamera camera(*K_, pose); + Point2 reprojectionError(camera.project(point, H1, H2) - z_); + return reprojectionError.vector(); + } + catch( CheiralityException& e) { + if (H1) *H1 = zeros(2,6); + if (H2) *H2 = zeros(2,3); + cout << e.what() << ": Landmark "<< this->key2_.index() << + " moved behind camera " << this->key1_.index() << endl; + return zero(2); + } } /** return the measurement */ diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt new file mode 100644 index 000000000..96f101f5c --- /dev/null +++ b/tests/CMakeLists.txt @@ -0,0 +1,29 @@ +include_directories( + 3rdparty/UFconfig + 3rdparty/CCOLAMD/Include + ${CMAKE_SOURCE_DIR}) + +find_package(Boost COMPONENTS serialization REQUIRED) + +# Build tests +file(GLOB tests_srcs "test*.cpp") +foreach(test_src ${tests_srcs}) + get_filename_component(test_base ${test_src} NAME_WE) + set( test_bin ${test_base} ) + add_executable(${test_bin} EXCLUDE_FROM_ALL ${test_src}) + add_dependencies(check ${test_bin}) + add_test(${test_bin} ${EXECUTABLE_OUTPUT_PATH}${test_bin}) + target_link_libraries(${test_bin} CppUnitLite gtsam ${Boost_SERIALIZATION_LIBRARY}) + add_custom_target(${test_base}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN}) +endforeach(test_src) + +# Build timing scripts +file(GLOB time_srcs "time*.cpp") +foreach(time_src ${time_srcs}) + get_filename_component(time_base ${time_src} NAME_WE) + set( time_bin ${time_base} ) + add_executable(${time_bin} EXCLUDE_FROM_ALL ${time_src}) + add_dependencies(check ${time_bin}) + target_link_libraries(${time_bin} CppUnitLite gtsam) + add_custom_target(${time_base}.run ${EXECUTABLE_OUTPUT_PATH}${time_bin} ${ARGN}) +endforeach(time_src) \ No newline at end of file diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 1610f98aa..329f0a643 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "Argument.h" @@ -70,8 +71,17 @@ string ArgumentList::types() const { /* ************************************************************************* */ string ArgumentList::signature() const { string str; + BOOST_FOREACH(Argument arg, *this) - str += arg.type[0]; + { + BOOST_FOREACH(char ch, arg.type) + if(isupper(ch)) + str += ch; + + if(str.length() == 0) + str += arg.type[0]; + } + return str; } diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt new file mode 100644 index 000000000..b6bf8b4f9 --- /dev/null +++ b/wrap/CMakeLists.txt @@ -0,0 +1,25 @@ +# Build/install Wrap + +# Build the executable itself +file(GLOB wrap_srcs "*.cpp") +list(REMOVE_ITEM wrap_srcs wrap.cpp) +add_library(wrapLib STATIC ${wrap_srcs}) +add_executable(wrap wrap.cpp) +target_link_libraries(wrap wrapLib) +install(TARGETS wrap DESTINATION ${CMAKE_INSTALL_PREFIX}/bin) + +# Build tests +file(GLOB wrap_test_srcs "tests/test*.cpp") +add_definitions(-DTOPSRCDIR="${CMAKE_SOURCE_DIR}") +foreach(test_src ${wrap_test_srcs} ) + get_filename_component(test_base ${test_src} NAME_WE) + set( test_bin wrap_${test_base} ) + add_executable(${test_bin} EXCLUDE_FROM_ALL ${test_src}) + add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}${test_bin}) + add_dependencies(check ${test_bin}) + target_link_libraries(${test_bin} CppUnitLite gtsam wrapLib) + add_custom_target(${test_bin}.run ${EXECUTABLE_OUTPUT_PATH}${test_bin} ${ARGN}) +endforeach(test_src) + +# Install matlab header +install(FILES matlab.h DESTINATION ${CMAKE_INSTALL_PREFIX}/include/wrap)