From 778001f63e3d58f58e55976393c2151e9370270e Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 13 Jun 2011 20:01:58 +0000 Subject: [PATCH] assorted cleanup - mostly comments and adding implementations to cpp files, rather than in header files --- CppUnitLite/Test.h | 4 + gtsam/base/Matrix.h | 11 - gtsam/base/timing.cpp | 192 ++++++++++++++++++ gtsam/base/timing.h | 179 ++-------------- gtsam/geometry/StereoPoint2.cpp | 9 + gtsam/geometry/StereoPoint2.h | 6 +- gtsam/inference/ClusterTree-inl.h | 8 + gtsam/inference/ClusterTree.h | 6 +- gtsam/inference/tests/testClusterTree.cpp | 5 + gtsam/inference/tests/testEliminationTree.cpp | 11 + gtsam/inference/tests/testFactorGraph.cpp | 7 +- gtsam/inference/tests/testInference.cpp | 7 +- gtsam/linear/GaussianConditional.cpp | 8 +- gtsam/linear/GaussianFactorGraph.cpp | 170 +++++++--------- gtsam/linear/GaussianFactorGraph.h | 10 - gtsam/linear/NoiseModel.h | 2 +- gtsam/linear/tests/testNoiseModel.cpp | 19 -- gtsam/nonlinear/NonlinearFactor.h | 12 +- gtsam/slam/GeneralSFMFactor.h | 11 + gtsam/slam/PartialPriorFactor.h | 11 + gtsam/slam/ProjectionFactor.h | 14 ++ tests/testGaussianFactorGraph.cpp | 26 ++- tests/testInference.cpp | 12 +- 23 files changed, 395 insertions(+), 345 deletions(-) diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index e6dafcc5b..1b236efca 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -113,6 +113,10 @@ protected: result_.addFailure (Failure (name_, __FILE__,__LINE__, SimpleString("Wrong exception: ") + StringFrom(#condition) + StringFrom(", expected: ") + StringFrom(#exception_name))); \ return; } } +#define EQUALITY(expected,actual)\ + { if (!assert_equal(expected,actual)) \ + result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); } + #define CHECK_EQUAL(expected,actual)\ { if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, StringFrom(expected), StringFrom(actual))); } diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 9ff6bc4ce..c3c8eb0f0 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -331,9 +331,6 @@ void householder(Matrix& A, size_t k); * @param unit, set true if unit triangular * @return the solution x of U*x=b */ -//FIXME: add back expression form -//template -//Vector backSubstituteUpper(const MATRIX& U, const VECTOR& b, bool unit=false); Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit=false); /** @@ -343,10 +340,7 @@ Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit=false); * @param unit, set true if unit triangular * @return the solution x of x'*U=b' */ -//FIXME: add back expression form //TODO: is this function necessary? it isn't used -//template -//Vector backSubstituteUpper(const VECTOR& b, const MATRIX& U, bool unit=false); Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit=false); /** @@ -453,11 +447,6 @@ DLT(const Matrix& A, double rank_tol = 1e-9); */ Matrix expm(const Matrix& A, size_t K=7); -// macro for unit tests -#define EQUALITY(expected,actual)\ - { if (!assert_equal(expected,actual)) \ - result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); } - } // namespace gtsam #include diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 06d2fed3e..fba804885 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -16,6 +16,14 @@ * @created Oct 5, 2010 */ +#include +#include +#include +#include +#include +#include + +#include #include boost::shared_ptr timingRoot(new TimingOutline("Total")); @@ -24,3 +32,187 @@ boost::weak_ptr timingCurrent(timingRoot); Timing timing; std::string timingPrefix; +/* ************************************************************************* */ +// Implementation of TimingOutline +/* ************************************************************************* */ + +/* ************************************************************************* */ +void TimingOutline::add(size_t usecs) { + t_ += usecs; + tIt_ += usecs; + ++ n_; +} + +/* ************************************************************************* */ +TimingOutline::TimingOutline(const std::string& label) : + t_(0), tIt_(0), tMax_(0), tMin_(0), n_(0), label_(label), timerActive_(false) {} + +/* ************************************************************************* */ +size_t TimingOutline::time() const { + size_t time = 0; + bool hasChildren = false; + BOOST_FOREACH(const boost::shared_ptr& child, children_) { + if(child) { + time += child->time(); + hasChildren = true; + } + } + if(hasChildren) + return time; + else + return t_; +} + +/* ************************************************************************* */ +void TimingOutline::print(const std::string& outline) const { + std::cout << outline << " " << label_ << ": " << double(t_)/1000000.0 << " (" << + n_ << " times, " << double(time())/1000000.0 << " children, min: " << double(tMin_)/1000000.0 << + " max: " << double(tMax_)/1000000.0 << ")\n"; + for(size_t i=0; i 0) + childOutline += "."; + childOutline += (boost::format("%d") % i).str(); +#else + childOutline += " "; +#endif + children_[i]->print(childOutline); + } + } +} + +/* ************************************************************************* */ +const boost::shared_ptr& TimingOutline::child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr) { + assert(thisPtr.lock().get() == this); + // Resize if necessary + if(child >= children_.size()) + children_.resize(child + 1); + // Create child if necessary + if(children_[child]) { +#ifndef NDEBUG + if(children_[child]->label_ != label) { + timingRoot->print(); + std::cerr << "gtsam timing: tic called with id=" << child << ", label=" << label << ", but this id already has the label " << children_[child]->label_ << std::endl; + exit(1); + } +#endif + } else { + children_[child].reset(new TimingOutline(label)); + children_[child]->parent_ = thisPtr; + } + return children_[child]; +} + +/* ************************************************************************* */ +void TimingOutline::tic() { + assert(!timerActive_); + timerActive_ = true; + gettimeofday(&t0_, NULL); +} + +/* ************************************************************************* */ +void TimingOutline::toc() { + struct timeval t; + gettimeofday(&t, NULL); + assert(timerActive_); + add(t.tv_sec*1000000 + t.tv_usec - (t0_.tv_sec*1000000 + t0_.tv_usec)); + timerActive_ = false; +} + +/* ************************************************************************* */ +void TimingOutline::finishedIteration() { + if(tIt_ > tMax_) + tMax_ = tIt_; + if(tMin_ == 0 || tIt_ < tMin_) + tMin_ = tIt_; + tIt_ = 0; + for(size_t i=0; ifinishedIteration(); +} + +/* ************************************************************************* */ +void tic_(size_t id, const std::string& label) { + if(ISDEBUG("timing-verbose")) + std::cout << "tic(" << id << ", " << label << ")" << std::endl; + boost::shared_ptr node = timingCurrent.lock()->child(id, label, timingCurrent); + timingCurrent = node; + node->tic(); +} + +/* ************************************************************************* */ +void toc_(size_t id) { + if(ISDEBUG("timing-verbose")) + std::cout << "toc(" << id << ")" << std::endl; + boost::shared_ptr current(timingCurrent.lock()); + if(!(id < current->parent_.lock()->children_.size() && current->parent_.lock()->children_[id] == current)) { + if(std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) + != current->parent_.lock()->children_.end()) + std::cout << "gtsam timing: Incorrect ID passed to toc, expected " + << std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) - current->parent_.lock()->children_.begin() + << ", got " << id << std::endl; + else + std::cout << "gtsam timing: Incorrect ID passed to toc, id " << id << " does not exist" << std::endl; + timingRoot->print(); + throw std::invalid_argument("gtsam timing: Incorrect ID passed to toc"); + } + current->toc(); + if(!current->parent_.lock()) { + std::cout << "gtsam timing: extra toc, already at the root" << std::endl; + timingRoot->print(); + throw std::invalid_argument("gtsam timing: extra toc, already at the root"); + } + timingCurrent = current->parent_; +} + +/* ************************************************************************* */ +void toc_(size_t id, const std::string& label) { + if(ISDEBUG("timing-verbose")) + std::cout << "toc(" << id << ", " << label << ")" << std::endl; + bool check = false; +#ifndef NDEBUG + // If NDEBUG is defined, still do this debug check if the granular debugging + // flag is enabled. If NDEBUG is not defined, always do this check. + check = true; +#endif + if(check || ISDEBUG("timing-debug")) { + if(label != timingCurrent.lock()->label_) { + std::cerr << "gtsam timing: toc called with id=" << id << ", label=\"" << label << "\", but expecting \"" << timingCurrent.lock()->label_ << "\"" << std::endl; + timingRoot->print(); + exit(1); + } + } + toc_(id); +} + +/* ************************************************************************* */ +// Timing class implementation +void Timing::print() { + std::map::iterator it; + for(it = this->stats.begin(); it!=stats.end(); it++) { + Stats& s = it->second; + printf("%s: %g (%i times, min: %g, max: %g)\n", + it->first.c_str(), s.t, s.n, s.t_min, s.t_max); + } +} + +/* ************************************************************************* */ +double _tic_() { + struct timeval t; + gettimeofday(&t, NULL); + return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.); +} + +/* ************************************************************************* */ +void ticPop_(const std::string& prefix, const std::string& id) { + toc_(id); + if(timingPrefix.size() < prefix.size()) { + fprintf(stderr, "Seems to be a mismatched push/pop in timing, exiting\n"); + exit(1); + } else if(timingPrefix.size() == prefix.size()) + timingPrefix.resize(0); + else + timingPrefix.resize(timingPrefix.size() - prefix.size() - 1); +} diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 6a7544c15..f1622e500 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -17,18 +17,11 @@ */ #pragma once -#include -#include #include -#include -#include #include -#include +#include #include #include -#include -#include - class TimingOutline; extern boost::shared_ptr timingRoot; @@ -47,151 +40,34 @@ protected: struct timeval t0_; bool timerActive_; - inline void add(size_t usecs) { - t_ += usecs; - tIt_ += usecs; - ++ n_; - } + void add(size_t usecs); public: - inline TimingOutline(const std::string& label) : - t_(0), tIt_(0), tMax_(0), tMin_(0), n_(0), label_(label), timerActive_(false) {} - inline size_t time() const { - size_t time = 0; - bool hasChildren = false; - BOOST_FOREACH(const boost::shared_ptr& child, children_) { - if(child) { - time += child->time(); - hasChildren = true; - } - } - if(hasChildren) - return time; - else - return t_; - } + TimingOutline(const std::string& label); - inline void print(const std::string& outline = "") const { - std::cout << outline << " " << label_ << ": " << double(t_)/1000000.0 << " (" << - n_ << " times, " << double(time())/1000000.0 << " children, min: " << double(tMin_)/1000000.0 << - " max: " << double(tMax_)/1000000.0 << ")\n"; - for(size_t i=0; i 0) - childOutline += "."; - childOutline += (boost::format("%d") % i).str(); - #else - childOutline += " "; - #endif - children_[i]->print(childOutline); - } - } - } + size_t time() const; - inline const boost::shared_ptr& child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr) { - assert(thisPtr.lock().get() == this); - // Resize if necessary - if(child >= children_.size()) - children_.resize(child + 1); - // Create child if necessary - if(children_[child]) { -#ifndef NDEBUG - if(children_[child]->label_ != label) { - timingRoot->print(); - std::cerr << "gtsam timing: tic called with id=" << child << ", label=" << label << ", but this id already has the label " << children_[child]->label_ << std::endl; - exit(1); - } -#endif - } else { - children_[child].reset(new TimingOutline(label)); - children_[child]->parent_ = thisPtr; - } - return children_[child]; - } + void print(const std::string& outline = "") const; - inline void tic() { - assert(!timerActive_); - timerActive_ = true; - gettimeofday(&t0_, NULL); - } + const boost::shared_ptr& child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr); - inline void toc() { - struct timeval t; - gettimeofday(&t, NULL); - assert(timerActive_); - add(t.tv_sec*1000000 + t.tv_usec - (t0_.tv_sec*1000000 + t0_.tv_usec)); - timerActive_ = false; - } + void tic(); - inline void finishedIteration() { - if(tIt_ > tMax_) - tMax_ = tIt_; - if(tMin_ == 0 || tIt_ < tMin_) - tMin_ = tIt_; - tIt_ = 0; - for(size_t i=0; ifinishedIteration(); - } + void toc(); + + void finishedIteration(); friend class AutoTimer; friend void toc_(size_t id); friend void toc_(size_t id, const std::string& label); -}; +}; // \TimingOutline -inline void tic_(size_t id, const std::string& label) { - if(ISDEBUG("timing-verbose")) - std::cout << "tic(" << id << ", " << label << ")" << std::endl; - boost::shared_ptr node = timingCurrent.lock()->child(id, label, timingCurrent); - timingCurrent = node; - node->tic(); -} +void tic_(size_t id, const std::string& label); -inline void toc_(size_t id) { - if(ISDEBUG("timing-verbose")) - std::cout << "toc(" << id << ")" << std::endl; - boost::shared_ptr current(timingCurrent.lock()); - if(!(id < current->parent_.lock()->children_.size() && current->parent_.lock()->children_[id] == current)) { - if(std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) - != current->parent_.lock()->children_.end()) - std::cout << "gtsam timing: Incorrect ID passed to toc, expected " - << std::find(current->parent_.lock()->children_.begin(), current->parent_.lock()->children_.end(), current) - current->parent_.lock()->children_.begin() - << ", got " << id << std::endl; - else - std::cout << "gtsam timing: Incorrect ID passed to toc, id " << id << " does not exist" << std::endl; - timingRoot->print(); - throw std::invalid_argument("gtsam timing: Incorrect ID passed to toc"); - } - current->toc(); - if(!current->parent_.lock()) { - std::cout << "gtsam timing: extra toc, already at the root" << std::endl; - timingRoot->print(); - throw std::invalid_argument("gtsam timing: extra toc, already at the root"); - } - timingCurrent = current->parent_; -} +void toc_(size_t id); -inline void toc_(size_t id, const std::string& label) { - if(ISDEBUG("timing-verbose")) - std::cout << "toc(" << id << ", " << label << ")" << std::endl; - bool check = false; -#ifndef NDEBUG - // If NDEBUG is defined, still do this debug check if the granular debugging - // flag is enabled. If NDEBUG is not defined, always do this check. - check = true; -#endif - if(check || ISDEBUG("timing-debug")) { - if(label != timingCurrent.lock()->label_) { - std::cerr << "gtsam timing: toc called with id=" << id << ", label=\"" << label << "\", but expecting \"" << timingCurrent.lock()->label_ << "\"" << std::endl; - timingRoot->print(); - exit(1); - } - } - toc_(id); -} +void toc_(size_t id, const std::string& label); inline void tictoc_finishedIteration_() { timingRoot->finishedIteration(); @@ -261,25 +137,15 @@ public: if (s.n==1 || s.t_max < dt) s.t_max = dt; if (s.n==1 || s.t_min > dt) s.t_min = dt; } - void print() { - std::map::iterator it; - for(it = stats.begin(); it!=stats.end(); it++) { - Stats& s = it->second; - printf("%s: %g (%i times, min: %g, max: %g)\n", - it->first.c_str(), s.t, s.n, s.t_min, s.t_max); - } - } + void print(); + double time(const std::string& id) { Stats& s = stats[id]; return s.t; } }; -inline double _tic_() { - struct timeval t; - gettimeofday(&t, NULL); - return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.); -} +double _tic_(); inline double _toc_(double t) { double s = _tic_(); return (std::max(0., s-t)); @@ -301,16 +167,7 @@ inline void ticPush_(const std::string& prefix, const std::string& id) { timingPrefix += prefix; tic_(id); } -inline void ticPop_(const std::string& prefix, const std::string& id) { - toc_(id); - if(timingPrefix.size() < prefix.size()) { - fprintf(stderr, "Seems to be a mismatched push/pop in timing, exiting\n"); - exit(1); - } else if(timingPrefix.size() == prefix.size()) - timingPrefix.resize(0); - else - timingPrefix.resize(timingPrefix.size() - prefix.size() - 1); -} +void ticPop_(const std::string& prefix, const std::string& id); inline void tictoc_print_() { timing.print(); timingRoot->print(); diff --git a/gtsam/geometry/StereoPoint2.cpp b/gtsam/geometry/StereoPoint2.cpp index 5eab59cc5..8e84f0e23 100644 --- a/gtsam/geometry/StereoPoint2.cpp +++ b/gtsam/geometry/StereoPoint2.cpp @@ -16,5 +16,14 @@ * Author: dellaert */ +#include #include +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +void StereoPoint2::print(const string& s) const { + cout << s << "(" << uL_ << ", " << uR_ << ", " << v_ << ")" << endl; +} +/* ************************************************************************* */ diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 225e342cf..a4ed3eb9a 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -18,7 +18,6 @@ #pragma once -#include #include namespace gtsam { @@ -45,10 +44,7 @@ namespace gtsam { } /** print */ - void print(const std::string& s) const { - std::cout << s << "(" << uL_ << ", " << uR_ << ", " << v_ << ")" - << std::endl; - } + void print(const std::string& s="") const; /** equals */ bool equals(const StereoPoint2& q, double tol=1e-9) const { diff --git a/gtsam/inference/ClusterTree-inl.h b/gtsam/inference/ClusterTree-inl.h index 87940fcd5..034aa46e5 100644 --- a/gtsam/inference/ClusterTree-inl.h +++ b/gtsam/inference/ClusterTree-inl.h @@ -19,6 +19,7 @@ #pragma once +#include #include #include @@ -94,6 +95,13 @@ namespace gtsam { * ClusterTree * ************************************************************************* */ template + void ClusterTree::print(const string& str) const { + cout << str << endl; + if (root_) root_->printTree(""); + } + + /* ************************************************************************* */ + template bool ClusterTree::equals(const ClusterTree& other, double tol) const { if (!root_ && !other.root_) return true; if (!root_ || !other.root_) return false; diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 18d1202d7..965bbdc2b 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -21,7 +21,6 @@ #include #include -#include #include #include @@ -104,10 +103,7 @@ namespace gtsam { sharedCluster root() const { return root_; } // print the object - void print(const std::string& str) const { - std::cout << str << std::endl; - if (root_) root_->printTree(""); - } + void print(const std::string& str="") const; /** check equality */ bool equals(const ClusterTree& other, double tol = 1e-9) const; diff --git a/gtsam/inference/tests/testClusterTree.cpp b/gtsam/inference/tests/testClusterTree.cpp index 7b47e23ae..ab7716c50 100644 --- a/gtsam/inference/tests/testClusterTree.cpp +++ b/gtsam/inference/tests/testClusterTree.cpp @@ -30,6 +30,11 @@ using namespace gtsam; template class ClusterTree; typedef ClusterTree SymbolicClusterTree; +/* ************************************************************************* */ +TEST(ClusterTree, constructor) { + SymbolicClusterTree tree; +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/inference/tests/testEliminationTree.cpp b/gtsam/inference/tests/testEliminationTree.cpp index 7477c1e66..8c59eb2e5 100644 --- a/gtsam/inference/tests/testEliminationTree.cpp +++ b/gtsam/inference/tests/testEliminationTree.cpp @@ -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 testEliminationTree.cpp * @brief diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp index 46484a426..e4960b861 100644 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -93,15 +93,12 @@ typedef boost::shared_ptr shared; // CHECK(singletonGraph_excepted.equals(singletonGraph)); //} +/* ************************************************************************* */ TEST(FactorGraph, dynamic_factor_cast) { FactorGraph fg; fg.dynamicCastFactors >(); } - /* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/inference/tests/testInference.cpp b/gtsam/inference/tests/testInference.cpp index 48dc8b5fa..df9ec4173 100644 --- a/gtsam/inference/tests/testInference.cpp +++ b/gtsam/inference/tests/testInference.cpp @@ -36,12 +36,7 @@ TEST(Inference, UnobservedVariables) { VariableIndex variableIndex(sfg); Permutation::shared_ptr colamd(Inference::PermutationCOLAMD(variableIndex)); - - //colamd->print("colamd: "); } /* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 25693e6af..959cfb41f 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -159,12 +159,11 @@ void GaussianConditional::rhs(VectorValues& x) const { void GaussianConditional::solveInPlace(VectorValues& x) const { static const bool debug = false; if(debug) print("Solving conditional in place"); -// Vector rhs(get_d()); Vector rhs = x.range(beginFrontals(), endFrontals()); for (const_iterator parent = beginParents(); parent != endParents(); ++parent) { rhs += -get_S(parent) * x[*parent]; } - Vector soln = permutation_.transpose()*backSubstituteUpper(get_R(), rhs, false); + Vector soln = permutation_.transpose() * get_R().triangularView().solve(rhs); if(debug) { gtsam::print(Matrix(get_R()), "Calling backSubstituteUpper on "); gtsam::print(rhs, "rhs: "); @@ -177,7 +176,6 @@ void GaussianConditional::solveInPlace(VectorValues& x) const { void GaussianConditional::solveInPlace(Permuted& x) const { static const bool debug = false; if(debug) print("Solving conditional in place (permuted)"); -// Vector rhs(get_d()); // Extract RHS from values - inlined from VectorValues size_t s = 0; for (const_iterator it=beginFrontals(); it!=endFrontals(); ++it) @@ -195,8 +193,8 @@ void GaussianConditional::solveInPlace(Permuted& x) const { rhs += -get_S(parent) * x[*parent]; } - // solve system - Vector soln = permutation_.transpose()*backSubstituteUpper(get_R(), rhs, false); + // solve system - backsubstitution + Vector soln = permutation_.transpose() * get_R().triangularView().solve(rhs); // apply solution: inlined manually due to permutation size_t solnStart = 0; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index d7761b761..587b3a471 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -30,8 +30,7 @@ using namespace gtsam; namespace gtsam { // Explicitly instantiate so we don't have to include everywhere - INSTANTIATE_FACTOR_GRAPH(GaussianFactor) - ; + INSTANTIATE_FACTOR_GRAPH(GaussianFactor); /* ************************************************************************* */ GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : @@ -120,31 +119,6 @@ namespace gtsam { return combined.matrix_augmented(); } - // VectorValues GaussianFactorGraph::allocateVectorValuesb() const { - // std::vector dimensions(size()) ; - // Index i = 0 ; - // BOOST_FOREACH( const sharedFactor& factor, factors_) { - // dimensions[i] = factor->numberOfRows() ; - // i++; - // } - // - // return VectorValues(dimensions) ; - // } - // - // void GaussianFactorGraph::getb(VectorValues &b) const { - // Index i = 0 ; - // BOOST_FOREACH( const sharedFactor& factor, factors_) { - // b[i] = factor->getb(); - // i++; - // } - // } - // - // VectorValues GaussianFactorGraph::getb() const { - // VectorValues b = allocateVectorValuesb() ; - // getb(b) ; - // return b ; - // } - /* ************************************************************************* */ // Helper functions for Combine static boost::tuple, size_t, size_t> countDims(const std::vector& factors, const VariableSlots& variableSlots) { @@ -419,7 +393,7 @@ namespace gtsam { toc(2, "Jacobian EliminateGaussian"); return make_pair(conditionals, factor); - } // EliminateQR + } // \EliminateQR /* ************************************************************************* */ GaussianFactorGraph::EliminationResult EliminatePreferCholesky( @@ -499,8 +473,7 @@ namespace gtsam { return ret; - } // EliminatePreferCholesky - + } // \EliminatePreferCholesky /* ************************************************************************* */ GaussianFactorGraph::EliminationResult EliminateLDL( @@ -537,7 +510,6 @@ namespace gtsam { cout << " Index: " << p.first << ", " << p.second.toString() << endl; } - HessianFactor::shared_ptr // combinedFactor(new HessianFactor(factors, dimensions, scatter)); toc(3, "combine"); @@ -561,86 +533,86 @@ namespace gtsam { combinedFactor->assertInvariants(); return make_pair(conditionals, combinedFactor); - } + } // \EliminateLDL /* ************************************************************************* */ - GaussianFactorGraph::EliminationResult EliminatePreferLDL( - const FactorGraph& factors, size_t nrFrontals) { + GaussianFactorGraph::EliminationResult EliminatePreferLDL( + const FactorGraph& factors, size_t nrFrontals) { - typedef JacobianFactor J; - typedef HessianFactor H; + typedef JacobianFactor J; + typedef HessianFactor H; - // If any JacobianFactors have constrained noise models, we have to convert - // all factors to JacobianFactors. Otherwise, we can convert all factors - // to HessianFactors. This is because QR can handle constrained noise - // models but LDL cannot. + // If any JacobianFactors have constrained noise models, we have to convert + // all factors to JacobianFactors. Otherwise, we can convert all factors + // to HessianFactors. This is because QR can handle constrained noise + // models but LDL cannot. - // Decide whether to use QR or LDL - // Check if any JacobianFactors have constrained noise models. - bool useQR = false; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); - if (jacobian && jacobian->get_model()->isConstrained()) { - useQR = true; - break; - } - } + // Decide whether to use QR or LDL + // Check if any JacobianFactors have constrained noise models. + bool useQR = false; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); + if (jacobian && jacobian->get_model()->isConstrained()) { + useQR = true; + break; + } + } - // Convert all factors to the appropriate type - // and call the type-specific EliminateGaussian. - if (useQR) return EliminateQR(factors, nrFrontals); + // Convert all factors to the appropriate type + // and call the type-specific EliminateGaussian. + if (useQR) return EliminateQR(factors, nrFrontals); - GaussianFactorGraph::EliminationResult ret; - try { - tic(2, "EliminateLDL"); - ret = EliminateLDL(factors, nrFrontals); - toc(2, "EliminateLDL"); - } catch (const exception& e) { - cout << "Exception in EliminateLDL: " << e.what() << endl; - SETDEBUG("EliminateLDL", true); - SETDEBUG("updateATA", true); - SETDEBUG("JacobianFactor::eliminate", true); - SETDEBUG("JacobianFactor::Combine", true); - SETDEBUG("ldlPartial", true); - SETDEBUG("findScatterAndDims", true); - factors.print("Combining factors: "); - EliminateLDL(factors, nrFrontals); - throw; - } + GaussianFactorGraph::EliminationResult ret; + try { + tic(2, "EliminateLDL"); + ret = EliminateLDL(factors, nrFrontals); + toc(2, "EliminateLDL"); + } catch (const exception& e) { + cout << "Exception in EliminateLDL: " << e.what() << endl; + SETDEBUG("EliminateLDL", true); + SETDEBUG("updateATA", true); + SETDEBUG("JacobianFactor::eliminate", true); + SETDEBUG("JacobianFactor::Combine", true); + SETDEBUG("ldlPartial", true); + SETDEBUG("findScatterAndDims", true); + factors.print("Combining factors: "); + EliminateLDL(factors, nrFrontals); + throw; + } - const bool checkLDL = ISDEBUG("EliminateGaussian Check LDL"); - if (checkLDL) { - GaussianFactorGraph::EliminationResult expected; - FactorGraph jacobians = convertToJacobians(factors); - try { - // Compare with QR - expected = EliminateJacobians(jacobians, nrFrontals); - } catch (...) { - cout << "Exception in QR" << endl; - throw; - } + const bool checkLDL = ISDEBUG("EliminateGaussian Check LDL"); + if (checkLDL) { + GaussianFactorGraph::EliminationResult expected; + FactorGraph jacobians = convertToJacobians(factors); + try { + // Compare with QR + expected = EliminateJacobians(jacobians, nrFrontals); + } catch (...) { + cout << "Exception in QR" << endl; + throw; + } - H actual_factor(*ret.second); - H expected_factor(*expected.second); - if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal( - expected_factor, actual_factor, 1.0)) { - cout << "LDL and QR do not agree" << endl; + H actual_factor(*ret.second); + H expected_factor(*expected.second); + if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal( + expected_factor, actual_factor, 1.0)) { + cout << "LDL and QR do not agree" << endl; - SETDEBUG("EliminateLDL", true); - SETDEBUG("updateATA", true); - SETDEBUG("JacobianFactor::eliminate", true); - SETDEBUG("JacobianFactor::Combine", true); - jacobians.print("Jacobian Factors: "); - EliminateJacobians(jacobians, nrFrontals); - EliminateLDL(factors, nrFrontals); - factors.print("Combining factors: "); + SETDEBUG("EliminateLDL", true); + SETDEBUG("updateATA", true); + SETDEBUG("JacobianFactor::eliminate", true); + SETDEBUG("JacobianFactor::Combine", true); + jacobians.print("Jacobian Factors: "); + EliminateJacobians(jacobians, nrFrontals); + EliminateLDL(factors, nrFrontals); + factors.print("Combining factors: "); - throw runtime_error("LDL and QR do not agree"); - } - } + throw runtime_error("LDL and QR do not agree"); + } + } - return ret; + return ret; - } // EliminatePreferLDL + } // \EliminatePreferLDL } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 1dcd8bc94..8f989c1d0 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -163,13 +163,6 @@ namespace gtsam { */ Matrix denseJacobian() const; - // get b -// void getb(VectorValues &b) const ; -// VectorValues getb() const ; -// -// // allocate a vectorvalues of b's structure -// VectorValues allocateVectorValuesb() const ; - private: /** Serialization function */ friend class boost::serialization::access; @@ -190,9 +183,6 @@ namespace gtsam { GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph< JacobianFactor>& factors, size_t nrFrontals = 1); -// GaussianFactorGraph::EliminationResult EliminateHessians(const FactorGraph< -// HessianFactor>& factors, size_t nrFrontals = 1); - GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph< GaussianFactor>& factors, size_t nrFrontals = 1); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 4f2fa4105..1bbf34dde 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -25,7 +25,7 @@ namespace gtsam { - class SharedDiagonal; // forward declare, defined at end + class SharedDiagonal; // forward declare namespace noiseModel { diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 68386e519..a0ac06847 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -208,23 +208,6 @@ TEST( NoiseModel, QR ) EXPECT(linear_dependent(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!! } -/* ************************************************************************* */ -//TEST( NoiseModel, QRColumnWise ) -//{ -// // Call Gaussian version -// Matrix Ab = exampleQR::Ab; // otherwise overwritten ! -// vector firstZeroRows; -// firstZeroRows += 0,1,2,3,4,5; // FD: no idea as not documented :-( -// SharedDiagonal actual = exampleQR::diagonal->QRColumnWise(Ab,firstZeroRows); -// SharedDiagonal expected = noiseModel::Unit::Create(4); -// EXPECT(assert_equal(*expected,*actual)); -// Matrix AbResized = ublas::triangular_adaptor(Ab); -// print(exampleQR::Rd, "Rd: "); -// print(Ab, "Ab: "); -// print(AbResized, "AbResized: "); -// EXPECT(linear_dependent(exampleQR::Rd,AbResized,1e-4)); // Ab was modified in place !!! -//} - /* ************************************************************************* */ TEST(NoiseModel, Cholesky) { @@ -234,8 +217,6 @@ TEST(NoiseModel, Cholesky) EXPECT(assert_equal(*expected,*actual)); // Ab was modified in place !!! Matrix actualRd = Ab.block(0, 0, actual->dim(), Ab.cols()).triangularView(); -// ublas::project(ublas::triangular_adaptor(Ab), -// ublas::range(0,actual->dim()), ublas::range(0, Ab.cols())); EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4)); } diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index b3fe82c8e..d92c14079 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -153,7 +153,7 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(noiseModel_); } - }; // NonlinearFactor + }; // \class NonlinearFactor /** @@ -268,7 +268,7 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(key_); } - }; + };// \class NonlinearFactor1 /** * A Gaussian nonlinear factor that takes 2 parameters @@ -394,7 +394,7 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(key2_); } - }; + }; // \class NonlinearFactor2 /* ************************************************************************* */ @@ -501,7 +501,7 @@ namespace gtsam { */ virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const { const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_]; - return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key2_], ordering[key3_])); + return IndexFactor::shared_ptr(new IndexFactor(var1, var2, var3)); } /** methods to retrieve keys */ @@ -539,8 +539,8 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(key3_); } - }; + }; // \class NonlinearFactor3 /* ************************************************************************* */ -} +} // \namespace gtsam diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index faae64d93..4a8a6f640 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.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 + + * -------------------------------------------------------------------------- */ + /* * GeneralSFMFactor.h * diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index 55df8c4ca..5aaeefddd 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.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 PartialPriorFactor.h * @brief A simple prior factor that allows for setting a prior only on a part of linear parameters diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index b936c6785..f097e8f2c 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -1,6 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * 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 ProjectionFactor.h * @brief Basic bearing factor from 2D measurement + * @author Chris Beall + * @author Richard Roberts + * @author Frank Dellaert * @author Alex Cunningham */ diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index b91a5fa5d..ddaf1b67a 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -306,8 +306,8 @@ TEST( GaussianFactorGraph, eliminateAll ) GaussianBayesNet actual = *GaussianSequentialSolver(fg1).eliminate(); EXPECT(assert_equal(expected,actual,tol)); - GaussianBayesNet actualET = *GaussianSequentialSolver(fg1).eliminate(); - EXPECT(assert_equal(expected,actualET,tol)); + GaussianBayesNet actualQR = *GaussianSequentialSolver(fg1, true).eliminate(); + EXPECT(assert_equal(expected,actualQR,tol)); } ///* ************************************************************************* */ @@ -473,7 +473,7 @@ TEST( GaussianFactorGraph, getOrdering) //} /* ************************************************************************* */ -TEST( GaussianFactorGraph, optimize ) +TEST( GaussianFactorGraph, optimize_LDL ) { // create an ordering Ordering ord; ord += "x2","l1","x1"; @@ -482,7 +482,25 @@ TEST( GaussianFactorGraph, optimize ) GaussianFactorGraph fg = createGaussianFactorGraph(ord); // optimize the graph - VectorValues actual = *GaussianSequentialSolver(fg).optimize(); + VectorValues actual = *GaussianSequentialSolver(fg, false).optimize(); + + // verify + VectorValues expected = createCorrectDelta(ord); + + EXPECT(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, optimize_QR ) +{ + // create an ordering + Ordering ord; ord += "x2","l1","x1"; + + // create a graph + GaussianFactorGraph fg = createGaussianFactorGraph(ord); + + // optimize the graph + VectorValues actual = *GaussianSequentialSolver(fg, true).optimize(); // verify VectorValues expected = createCorrectDelta(ord); diff --git a/tests/testInference.cpp b/tests/testInference.cpp index 4f4428af1..1f6ccbdca 100644 --- a/tests/testInference.cpp +++ b/tests/testInference.cpp @@ -43,11 +43,10 @@ TEST(GaussianFactorGraph, createSmoother) // eliminate vector x3var; x3var.push_back(ordering["x3"]); vector x1var; x1var.push_back(ordering["x1"]); - // NOTE: fails when using LDL, works with QR GaussianBayesNet p_x3 = *GaussianSequentialSolver( - *GaussianSequentialSolver(fg2).jointFactorGraph(x3var), true).eliminate(); + *GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate(); GaussianBayesNet p_x1 = *GaussianSequentialSolver( - *GaussianSequentialSolver(fg2).jointFactorGraph(x1var), true).eliminate(); + *GaussianSequentialSolver(fg2).jointFactorGraph(x1var)).eliminate(); CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry } @@ -58,10 +57,8 @@ TEST( Inference, marginals ) // create and marginalize a small Bayes net on "x" GaussianBayesNet cbn = createSmallGaussianBayesNet(); vector xvar; xvar.push_back(0); - // NOTE: fails when using LDL, works with QR GaussianBayesNet actual = *GaussianSequentialSolver( - *GaussianSequentialSolver( - GaussianFactorGraph(cbn), true).jointFactorGraph(xvar), true).eliminate(); + *GaussianSequentialSolver(GaussianFactorGraph(cbn)).jointFactorGraph(xvar)).eliminate(); // expected is just scalar Gaussian on x GaussianBayesNet expected = scalarGaussian(0, 4, sqrt(2)); @@ -92,8 +89,7 @@ TEST( Inference, marginals2) Ordering ordering(*fg.orderingCOLAMD(init)); FactorGraph::shared_ptr gfg(fg.linearize(init, ordering)); - // NOTE: fails when using LDL, works with QR - GaussianMultifrontalSolver solver(*gfg, true); + GaussianMultifrontalSolver solver(*gfg); solver.marginalFactor(ordering[PointKey(0)]); }