diff --git a/.cproject b/.cproject index b74d762a0..2a3996bb7 100644 --- a/.cproject +++ b/.cproject @@ -353,9 +353,9 @@ true true - + make -testConditionalGaussian.run +testGaussianConditional.run true true true diff --git a/cpp/BayesNet.h b/cpp/BayesNet.h index f62b1462e..75d9a1f5d 100644 --- a/cpp/BayesNet.h +++ b/cpp/BayesNet.h @@ -23,7 +23,7 @@ namespace gtsam { * Bayes network * This is the base class for SymbolicBayesNet, DiscreteBayesNet, and GaussianBayesNet * corresponding to what is used for the "Conditional" template argument: - * a SymbolicConditional, ConditionalProbabilityTable, or a ConditionalGaussian + * a SymbolicConditional, ConditionalProbabilityTable, or a GaussianConditional */ template class BayesNet: public Testable > { diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index fcd73eda6..0b094d68b 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -23,7 +23,7 @@ namespace gtsam { /** * Bayes tree * Templated on the Conditional class, the type of node in the underlying Bayes chain. - * This could be a ConditionalProbabilityTable, a ConditionalGaussian, or a SymbolicConditional + * This could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional */ template class BayesTree: public Testable > { diff --git a/cpp/GaussianBayesNet.cpp b/cpp/GaussianBayesNet.cpp index fd1e24d43..f4f201310 100644 --- a/cpp/GaussianBayesNet.cpp +++ b/cpp/GaussianBayesNet.cpp @@ -16,7 +16,7 @@ using namespace gtsam; // Explicitly instantiate so we don't have to include everywhere #include "BayesNet-inl.h" -template class BayesNet; +template class BayesNet; // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) @@ -27,8 +27,8 @@ namespace gtsam { /* ************************************************************************* */ GaussianBayesNet scalarGaussian(const string& key, double mu, double sigma) { GaussianBayesNet bn; - ConditionalGaussian::shared_ptr - conditional(new ConditionalGaussian(key, Vector_(1,mu), eye(1), Vector_(1,sigma))); + GaussianConditional::shared_ptr + conditional(new GaussianConditional(key, Vector_(1,mu), eye(1), Vector_(1,sigma))); bn.push_back(conditional); return bn; } @@ -37,8 +37,8 @@ GaussianBayesNet scalarGaussian(const string& key, double mu, double sigma) { GaussianBayesNet simpleGaussian(const string& key, const Vector& mu, double sigma) { GaussianBayesNet bn; size_t n = mu.size(); - ConditionalGaussian::shared_ptr - conditional(new ConditionalGaussian(key, mu, eye(n), repeat(n,sigma))); + GaussianConditional::shared_ptr + conditional(new GaussianConditional(key, mu, eye(n), repeat(n,sigma))); bn.push_back(conditional); return bn; } @@ -46,14 +46,14 @@ GaussianBayesNet simpleGaussian(const string& key, const Vector& mu, double sigm /* ************************************************************************* */ void push_front(GaussianBayesNet& bn, const string& key, Vector d, Matrix R, const string& name1, Matrix S, Vector sigmas) { - ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key, d, R, name1, S, sigmas)); + GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, sigmas)); bn.push_front(cg); } /* ************************************************************************* */ void push_front(GaussianBayesNet& bn, const string& key, Vector d, Matrix R, const string& name1, Matrix S, const string& name2, Matrix T, Vector sigmas) { - ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key, d, R, name1, S, name2, T, sigmas)); + GaussianConditional::shared_ptr cg(new GaussianConditional(key, d, R, name1, S, name2, T, sigmas)); bn.push_front(cg); } @@ -63,7 +63,7 @@ VectorConfig optimize(const GaussianBayesNet& bn) VectorConfig result; /** solve each node in turn in topological sort order (parents first)*/ - BOOST_REVERSE_FOREACH(ConditionalGaussian::shared_ptr cg, bn) { + BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, bn) { Vector x = cg->solve(result); // Solve for that variable result.insert(cg->key(),x); // store result in partial solution } @@ -76,7 +76,7 @@ pair matrix(const GaussianBayesNet& bn) { // add the dimensions of all variables to get matrix dimension // and at the same time create a mapping from keys to indices size_t N=0; map mapping; - BOOST_FOREACH(ConditionalGaussian::shared_ptr cg,bn) { + BOOST_FOREACH(GaussianConditional::shared_ptr cg,bn) { mapping.insert(make_pair(cg->key(),N)); N += cg->dim(); } @@ -87,7 +87,7 @@ pair matrix(const GaussianBayesNet& bn) { string key; size_t I; FOREACH_PAIR(key,I,mapping) { // find corresponding conditional - ConditionalGaussian::shared_ptr cg = bn[key]; + GaussianConditional::shared_ptr cg = bn[key]; // get RHS and copy to d const Vector& d_ = cg->get_d(); @@ -102,7 +102,7 @@ pair matrix(const GaussianBayesNet& bn) { R(I+i,I+j) = R_(i,j); // loop over S matrices and copy them into R - ConditionalGaussian::const_iterator keyS = cg->parentsBegin(); + GaussianConditional::const_iterator keyS = cg->parentsBegin(); for (; keyS!=cg->parentsEnd(); keyS++) { Matrix S = keyS->second; // get S matrix const size_t m = S.size1(), n = S.size2(); // find S size diff --git a/cpp/GaussianBayesNet.h b/cpp/GaussianBayesNet.h index e013fe64c..a5cdaa7f8 100644 --- a/cpp/GaussianBayesNet.h +++ b/cpp/GaussianBayesNet.h @@ -11,13 +11,13 @@ #include -#include "ConditionalGaussian.h" +#include "GaussianConditional.h" #include "BayesNet.h" namespace gtsam { /** A Bayes net made from linear-Gaussian densities */ - typedef BayesNet GaussianBayesNet; + typedef BayesNet GaussianBayesNet; /** Create a scalar Gaussian */ GaussianBayesNet scalarGaussian(const std::string& key, double mu=0.0, double sigma=1.0); diff --git a/cpp/ConditionalGaussian.cpp b/cpp/GaussianConditional.cpp similarity index 83% rename from cpp/ConditionalGaussian.cpp rename to cpp/GaussianConditional.cpp index 124c4fb70..fa6fbb570 100644 --- a/cpp/ConditionalGaussian.cpp +++ b/cpp/GaussianConditional.cpp @@ -1,5 +1,5 @@ /** - * @file ConditionalGaussian.cpp + * @file GaussianConditional.cpp * @brief Conditional Gaussian Base class * @author Christian Potthast */ @@ -7,26 +7,26 @@ #include #include #include "Ordering.h" -#include "ConditionalGaussian.h" +#include "GaussianConditional.h" using namespace std; using namespace gtsam; /* ************************************************************************* */ -ConditionalGaussian::ConditionalGaussian(const string& key,Vector d, Matrix R, Vector sigmas) : +GaussianConditional::GaussianConditional(const string& key,Vector d, Matrix R, Vector sigmas) : Conditional (key), R_(R),sigmas_(sigmas),d_(d) { } /* ************************************************************************* */ -ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R, +GaussianConditional::GaussianConditional(const string& key, Vector d, Matrix R, const string& name1, Matrix S, Vector sigmas) : Conditional (key), R_(R), sigmas_(sigmas), d_(d) { parents_.insert(make_pair(name1, S)); } /* ************************************************************************* */ -ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R, +GaussianConditional::GaussianConditional(const string& key, Vector d, Matrix R, const string& name1, Matrix S, const string& name2, Matrix T, Vector sigmas) : Conditional (key), R_(R),sigmas_(sigmas), d_(d) { parents_.insert(make_pair(name1, S)); @@ -34,13 +34,13 @@ ConditionalGaussian::ConditionalGaussian(const string& key, Vector d, Matrix R, } /* ************************************************************************* */ -ConditionalGaussian::ConditionalGaussian(const string& key, +GaussianConditional::GaussianConditional(const string& key, const Vector& d, const Matrix& R, const map& parents, Vector sigmas) : Conditional (key), R_(R),sigmas_(sigmas), d_(d), parents_(parents) { } /* ************************************************************************* */ -void ConditionalGaussian::print(const string &s) const +void GaussianConditional::print(const string &s) const { cout << s << ": density on " << key_ << endl; gtsam::print(R_,"R"); @@ -54,9 +54,9 @@ void ConditionalGaussian::print(const string &s) const } /* ************************************************************************* */ -bool ConditionalGaussian::equals(const Conditional &c, double tol) const { +bool GaussianConditional::equals(const Conditional &c, double tol) const { if (!Conditional::equals(c)) return false; - const ConditionalGaussian* p = dynamic_cast (&c); + const GaussianConditional* p = dynamic_cast (&c); if (p == NULL) return false; Parents::const_iterator it = parents_.begin(); @@ -85,7 +85,7 @@ bool ConditionalGaussian::equals(const Conditional &c, double tol) const { } /* ************************************************************************* */ -list ConditionalGaussian::parents() const { +list GaussianConditional::parents() const { list result; for (Parents::const_iterator it = parents_.begin(); it != parents_.end(); it++) result.push_back(it->first); @@ -93,7 +93,7 @@ list ConditionalGaussian::parents() const { } /* ************************************************************************* */ -Vector ConditionalGaussian::solve(const VectorConfig& x) const { +Vector GaussianConditional::solve(const VectorConfig& x) const { Vector rhs = d_; for (Parents::const_iterator it = parents_.begin(); it != parents_.end(); it++) { diff --git a/cpp/ConditionalGaussian.h b/cpp/GaussianConditional.h similarity index 84% rename from cpp/ConditionalGaussian.h rename to cpp/GaussianConditional.h index fcfd6afe4..ed6036ab1 100644 --- a/cpp/ConditionalGaussian.h +++ b/cpp/GaussianConditional.h @@ -1,5 +1,5 @@ /** - * @file ConditionalGaussian.h + * @file GaussianConditional.h * @brief Conditional Gaussian Base class * @author Christian Potthast */ @@ -28,12 +28,12 @@ class Ordering; * It has a set of parents y,z, etc. and implements a probability density on x. * The negative log-probability is given by || Rx - (d - Sy - Tz - ...)||^2 */ -class ConditionalGaussian : public Conditional { +class GaussianConditional : public Conditional { public: typedef std::map Parents; typedef Parents::const_iterator const_iterator; - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; protected: @@ -52,41 +52,41 @@ protected: public: /** default constructor needed for serialization */ - ConditionalGaussian(){} + GaussianConditional(){} /** constructor */ - ConditionalGaussian(const std::string& key) : + GaussianConditional(const std::string& key) : Conditional (key) {} /** constructor with no parents * |Rx-d| */ - ConditionalGaussian(const std::string& key, Vector d, Matrix R, Vector sigmas); + GaussianConditional(const std::string& key, Vector d, Matrix R, Vector sigmas); /** constructor with only one parent * |Rx+Sy-d| */ - ConditionalGaussian(const std::string& key, Vector d, Matrix R, + GaussianConditional(const std::string& key, Vector d, Matrix R, const std::string& name1, Matrix S, Vector sigmas); /** constructor with two parents * |Rx+Sy+Tz-d| */ - ConditionalGaussian(const std::string& key, Vector d, Matrix R, + GaussianConditional(const std::string& key, Vector d, Matrix R, const std::string& name1, Matrix S, const std::string& name2, Matrix T, Vector sigmas); /** * constructor with number of arbitrary parents * |Rx+sum(Ai*xi)-d| */ - ConditionalGaussian(const std::string& key, const Vector& d, + GaussianConditional(const std::string& key, const Vector& d, const Matrix& R, const Parents& parents, Vector sigmas); /** deconstructor */ - virtual ~ConditionalGaussian() {} + virtual ~GaussianConditional() {} /** print */ - void print(const std::string& = "ConditionalGaussian") const; + void print(const std::string& = "GaussianConditional") const; /** equals function */ bool equals(const Conditional &cg, double tol = 1e-9) const; @@ -97,7 +97,7 @@ public: /** return all parents */ std::list parents() const; - /** return stuff contained in ConditionalGaussian */ + /** return stuff contained in GaussianConditional */ const Vector& get_d() const {return d_;} const Matrix& get_R() const {return R_;} const Vector& get_sigmas() const {return sigmas_;} diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp index d248c5e90..97418d585 100644 --- a/cpp/GaussianFactor.cpp +++ b/cpp/GaussianFactor.cpp @@ -11,7 +11,7 @@ #include "Matrix.h" #include "Ordering.h" -#include "ConditionalGaussian.h" +#include "GaussianConditional.h" #include "GaussianFactor.h" using namespace std; @@ -26,7 +26,7 @@ using namespace gtsam; typedef pair& mypair; /* ************************************************************************* */ -GaussianFactor::GaussianFactor(const boost::shared_ptr& cg) : +GaussianFactor::GaussianFactor(const boost::shared_ptr& cg) : b_(cg->get_d()) { As_.insert(make_pair(cg->key(), cg->get_R())); std::map::const_iterator it = cg->parentsBegin(); @@ -282,11 +282,11 @@ void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, const size_t m, /* Note, in place !!!! * Do incomplete QR factorization for the first n columns * We will do QR on all matrices and on RHS - * Then take first n rows and make a ConditionalGaussian, + * Then take first n rows and make a GaussianConditional, * and last rows to make a new joint linear factor on separator */ /* ************************************************************************* */ -pair +pair GaussianFactor::eliminate(const string& key) const { bool verbose = false; @@ -297,7 +297,7 @@ GaussianFactor::eliminate(const string& key) const if (it==As_.end()) { // Conditional Gaussian is just a parent-less node with P(x)=1 GaussianFactor::shared_ptr lf(new GaussianFactor); - ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key)); + GaussianConditional::shared_ptr cg(new GaussianConditional(key)); return make_pair(cg,lf); } @@ -339,7 +339,7 @@ GaussianFactor::eliminate(const string& key) const } // create base conditional Gaussian - ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(key, + GaussianConditional::shared_ptr cg(new GaussianConditional(key, sub(d, 0, n1), // form d vector sub(R, 0, n1, 0, n1), // form R matrix sub(newSigmas, 0, n1))); // get standard deviations diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index 54e5c5dae..5c57b2b88 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -19,7 +19,7 @@ namespace gtsam { - class ConditionalGaussian; + class GaussianConditional; class Ordering; /** @@ -95,7 +95,7 @@ public: } /** Construct from Conditional Gaussian */ - GaussianFactor(const boost::shared_ptr& cg); + GaussianFactor(const boost::shared_ptr& cg); /** * Constructor that combines a set of factors @@ -232,7 +232,7 @@ public: * @param key the key of the node to be eliminated * @return a new factor and a conditional gaussian on the eliminated variable */ - std::pair, shared_ptr> + std::pair, shared_ptr> eliminate(const std::string& key) const; /** diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index c7fe3ef56..edfb72364 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -42,9 +42,9 @@ set GaussianFactorGraph::find_separator(const string& key) const } /* ************************************************************************* */ -ConditionalGaussian::shared_ptr +GaussianConditional::shared_ptr GaussianFactorGraph::eliminateOne(const std::string& key) { - return gtsam::eliminateOne(*this, key); + return gtsam::eliminateOne(*this, key); } /* ************************************************************************* */ @@ -53,7 +53,7 @@ GaussianFactorGraph::eliminate(const Ordering& ordering) { GaussianBayesNet chordalBayesNet; // empty BOOST_FOREACH(string key, ordering) { - ConditionalGaussian::shared_ptr cg = eliminateOne(key); + GaussianConditional::shared_ptr cg = eliminateOne(key); chordalBayesNet.push_back(cg); } return chordalBayesNet; @@ -75,7 +75,7 @@ GaussianFactorGraph::eliminate_(const Ordering& ordering) { boost::shared_ptr chordalBayesNet(new GaussianBayesNet); // empty BOOST_FOREACH(string key, ordering) { - ConditionalGaussian::shared_ptr cg = eliminateOne(key); + GaussianConditional::shared_ptr cg = eliminateOne(key); chordalBayesNet->push_back(cg); } return chordalBayesNet; diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index 68d494ed8..807bca1c2 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -66,7 +66,7 @@ namespace gtsam { * Eliminates the factors from the factor graph through findAndRemoveFactors * and adds a new factor on the separator to the factor graph */ - ConditionalGaussian::shared_ptr eliminateOne(const std::string& key); + GaussianConditional::shared_ptr eliminateOne(const std::string& key); /** * eliminate factor graph in place(!) in the given order, yielding diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 07744f360..0445d8afe 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -85,16 +85,16 @@ testSymbolicBayesNet_LDADD = libgtsam.la # Gaussian inference headers += GaussianFactorSet.h -sources += VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp ConditionalGaussian.cpp GaussianBayesNet.cpp -check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testConditionalGaussian testGaussianBayesNet +sources += VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp +check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testVectorConfig_SOURCES = testVectorConfig.cpp testVectorConfig_LDADD = libgtsam.la testGaussianFactor_SOURCES = $(example) testGaussianFactor.cpp testGaussianFactor_LDADD = libgtsam.la testGaussianFactorGraph_SOURCES = $(example) testGaussianFactorGraph.cpp testGaussianFactorGraph_LDADD = libgtsam.la -testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp -testConditionalGaussian_LDADD = libgtsam.la +testGaussianConditional_SOURCES = $(example) testGaussianConditional.cpp +testGaussianConditional_LDADD = libgtsam.la testGaussianBayesNet_SOURCES = $(example) testGaussianBayesNet.cpp testGaussianBayesNet_LDADD = libgtsam.la @@ -112,13 +112,16 @@ timeGaussianFactorGraph: LDFLAGS += smallExample.o -L.libs -lgtsam -L../CppUnitL headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h sources += NonlinearFactor.cpp -check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer +sources += NonlinearEquality.cpp +check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp testNonlinearFactor_LDADD = libgtsam.la testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp testNonlinearFactorGraph_LDADD = libgtsam.la testNonlinearOptimizer_SOURCES = $(example) testNonlinearOptimizer.cpp testNonlinearOptimizer_LDADD = libgtsam.la +testNonlinearEquality_SOURCES = testNonlinearEquality.cpp +testNonlinearEquality_LDADD = libgtsam.la # geometry sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp diff --git a/cpp/NonlinearEquality.cpp b/cpp/NonlinearEquality.cpp new file mode 100644 index 000000000..6fbaf0f6d --- /dev/null +++ b/cpp/NonlinearEquality.cpp @@ -0,0 +1,9 @@ +/* + * @file NonlinearEquality.cpp + * @brief + * @author alexgc + */ + +#include "NonlinearEquality.h" + + diff --git a/cpp/NonlinearEquality.h b/cpp/NonlinearEquality.h new file mode 100644 index 000000000..5a1a504e9 --- /dev/null +++ b/cpp/NonlinearEquality.h @@ -0,0 +1,26 @@ +/* + * @file NonlinearEquality.h + * @brief Factor to handle enforced equality between factors + * @author Alex Cunningham + */ + +#pragma once + +#include "NonlinearFactor.h" + +namespace gtsam { + +/** + * An equality factor that forces either one variable to a constant, + * or a set of variables to be equal to each other. + * Throws an error at linearization if the constraints are not met. + */ +template +class NonlinearEquality : public NonlinearFactor{ +public: + NonlinearEquality(); + virtual ~NonlinearEquality(); +}; + +} + diff --git a/cpp/gtsam.h b/cpp/gtsam.h index 623c54944..6be6041d5 100644 --- a/cpp/gtsam.h +++ b/cpp/gtsam.h @@ -52,19 +52,19 @@ class GaussianFactor { pair matrix(const Ordering& ordering) const; }; -class ConditionalGaussian { - ConditionalGaussian(); - ConditionalGaussian(string key, +class GaussianConditional { + GaussianConditional(); + GaussianConditional(string key, Vector d, Matrix R, Vector sigmas); - ConditionalGaussian(string key, + GaussianConditional(string key, Vector d, Matrix R, string name1, Matrix S, Vector sigmas); - ConditionalGaussian(string key, + GaussianConditional(string key, Vector d, Matrix R, string name1, @@ -75,15 +75,15 @@ class ConditionalGaussian { void print(string s) const; Vector solve(const VectorConfig& x); void add(string key, Matrix S); - bool equals(const ConditionalGaussian &cg, double tol) const; + bool equals(const GaussianConditional &cg, double tol) const; }; class GaussianBayesNet { GaussianBayesNet(); void print(string s) const; bool equals(const GaussianBayesNet& cbn, double tol) const; - void push_back(ConditionalGaussian* conditional); - void push_front(ConditionalGaussian* conditional); + void push_back(GaussianConditional* conditional); + void push_front(GaussianConditional* conditional); }; class GaussianFactorGraph { @@ -96,7 +96,7 @@ class GaussianFactorGraph { void print(string s) const; bool equals(const GaussianFactorGraph& lfgraph, double tol) const; - ConditionalGaussian* eliminateOne(string key); + GaussianConditional* eliminateOne(string key); GaussianBayesNet* eliminate_(const Ordering& ordering); VectorConfig* optimize_(const Ordering& ordering); pair matrix(const Ordering& ordering) const; diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index 52bfd90e7..e08469334 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -204,9 +204,9 @@ GaussianBayesNet createSmallGaussianBayesNet() Vector tau(1); tau(0) = 1.0; // define nodes and specify in reverse topological sort (i.e. parents last) - ConditionalGaussian::shared_ptr - Px_y(new ConditionalGaussian("x",d1,R11,"y",S12,tau)), - Py(new ConditionalGaussian("y",d2,R22,tau)); + GaussianConditional::shared_ptr + Px_y(new GaussianConditional("x",d1,R11,"y",S12,tau)), + Py(new GaussianConditional("y",d2,R22,tau)); GaussianBayesNet cbn; cbn.push_back(Px_y); cbn.push_back(Py); @@ -502,11 +502,11 @@ VectorConfig createMultiConstraintConfig() { // Matrix R = eye(2); // Vector d = c["x1"]; // double sigma = 0.1; -// ConditionalGaussian::shared_ptr f1(new ConditionalGaussian(d/sigma, R/sigma)); +// GaussianConditional::shared_ptr f1(new GaussianConditional(d/sigma, R/sigma)); // cbn.insert("x1", f1); // // // add a delta function to the cbn -// ConstrainedConditionalGaussian::shared_ptr f2(new ConstrainedConditionalGaussian); //(c["x0"], "x0")); +// ConstrainedGaussianConditional::shared_ptr f2(new ConstrainedGaussianConditional); //(c["x0"], "x0")); // cbn.insert_df("x0", f2); // // return cbn; diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index b670718e7..cfa445672 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -17,7 +17,7 @@ using namespace boost::assign; using namespace gtsam; -typedef BayesTree Gaussian; +typedef BayesTree Gaussian; // Conditionals for ASIA example from the tutorial with A and D evidence SymbolicConditional::shared_ptr B(new SymbolicConditional("B")), L( @@ -226,7 +226,7 @@ TEST( BayesTree, balanced_smoother_shortcuts ) CHECK(assert_equal(empty,actual2,1e-4)); // Check the conditional P(C3|Root), which should be equal to P(x2|x4) - ConditionalGaussian::shared_ptr p_x2_x4 = chordalBayesNet["x2"]; + GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet["x2"]; GaussianBayesNet expected3; expected3.push_back(p_x2_x4); Gaussian::sharedClique C3 = bayesTree["x1"]; GaussianBayesNet actual3 = C3->shortcut(R); @@ -252,7 +252,7 @@ TEST( BayesTree, balanced_smoother_clique_marginals ) push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma); Gaussian::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"]; FactorGraph marginal = C3->marginal(R); - GaussianBayesNet actual = eliminate(marginal,C3->keys()); + GaussianBayesNet actual = eliminate(marginal,C3->keys()); CHECK(assert_equal(expected,actual,1e-4)); } diff --git a/cpp/testGaussianBayesNet.cpp b/cpp/testGaussianBayesNet.cpp index 39a2376b8..2cba16787 100644 --- a/cpp/testGaussianBayesNet.cpp +++ b/cpp/testGaussianBayesNet.cpp @@ -42,7 +42,7 @@ TEST( GaussianBayesNet, constructor ) sigmas(0) = 1.; // define nodes and specify in reverse topological sort (i.e. parents last) - ConditionalGaussian x("x",d1,R11,"y",S12, sigmas), y("y",d2,R22, sigmas); + GaussianConditional x("x",d1,R11,"y",S12, sigmas), y("y",d2,R22, sigmas); // check small example which uses constructor GaussianBayesNet cbn = createSmallGaussianBayesNet(); diff --git a/cpp/testConditionalGaussian.cpp b/cpp/testGaussianConditional.cpp similarity index 90% rename from cpp/testConditionalGaussian.cpp rename to cpp/testGaussianConditional.cpp index 35c6b4b63..9b8839e3a 100644 --- a/cpp/testConditionalGaussian.cpp +++ b/cpp/testGaussianConditional.cpp @@ -1,5 +1,5 @@ /** - * @file testConditionalGaussian.cpp + * @file testGaussianConditional.cpp * @brief Unit tests for Conditional gaussian * @author Christian Potthast **/ @@ -15,14 +15,14 @@ #endif //HAVE_BOOST_SERIALIZATION #include "Matrix.h" -#include "ConditionalGaussian.h" +#include "GaussianConditional.h" using namespace gtsam; /* ************************************************************************* */ /* unit test for equals */ /* ************************************************************************* */ -TEST( ConditionalGaussian, equals ) +TEST( GaussianConditional, equals ) { // create a conditional gaussian node Matrix A1(2,2); @@ -44,7 +44,7 @@ TEST( ConditionalGaussian, equals ) Vector d(2); d(0) = 0.2; d(1) = 0.5; - ConditionalGaussian + GaussianConditional expected("x",d, R, "x1", A1, "l1", A2, tau), actual("x",d, R, "x1", A1, "l1", A2, tau); @@ -55,7 +55,7 @@ TEST( ConditionalGaussian, equals ) /* ************************************************************************* */ /* unit test for solve */ /* ************************************************************************* */ -TEST( ConditionalGaussian, solve ) +TEST( GaussianConditional, solve ) { //expected solution Vector expected(2); @@ -76,7 +76,7 @@ TEST( ConditionalGaussian, solve ) Vector tau = ones(2); - ConditionalGaussian cg("x",d, R, "x1", A1, "l1", A2, tau); + GaussianConditional cg("x",d, R, "x1", A1, "l1", A2, tau); Vector sx1(2); sx1(0) = 1.0; sx1(1) = 1.0; @@ -98,7 +98,7 @@ TEST( ConditionalGaussian, solve ) /* unit test for serialization */ /* ************************************************************************* */ #ifdef HAVE_BOOST_SERIALIZATION -TEST( ConditionalGaussian, serialize ) +TEST( GaussianConditional, serialize ) { // create a conditional gaussion node Matrix A1(2,2); @@ -116,7 +116,7 @@ TEST( ConditionalGaussian, serialize ) Vector d(2); d(0) = 0.2; d(1) = 0.5; - ConditionalGaussian cg("x2", d, R, "x1", A1, "l1", A2); + GaussianConditional cg("x2", d, R, "x1", A1, "l1", A2); //serialize the CG std::ostringstream in_archive_stream; @@ -127,7 +127,7 @@ TEST( ConditionalGaussian, serialize ) //deserialize the CGg std::istringstream out_archive_stream(serialized); boost::archive::text_iarchive out_archive(out_archive_stream); - ConditionalGaussian output; + GaussianConditional output; out_archive >> output; //check for equality diff --git a/cpp/testGaussianFactor.cpp b/cpp/testGaussianFactor.cpp index f48ae5a3f..1413f0ffe 100644 --- a/cpp/testGaussianFactor.cpp +++ b/cpp/testGaussianFactor.cpp @@ -17,7 +17,7 @@ using namespace boost::assign; #include "Matrix.h" #include "Ordering.h" -#include "ConditionalGaussian.h" +#include "GaussianConditional.h" #include "smallExample.h" using namespace std; @@ -314,7 +314,7 @@ TEST( GaussianFactor, eliminate ) GaussianFactor combined(lfg); // eliminate the combined factor - ConditionalGaussian::shared_ptr actualCG; + GaussianConditional::shared_ptr actualCG; GaussianFactor::shared_ptr actualLF; boost::tie(actualCG,actualLF) = combined.eliminate("x2"); @@ -338,7 +338,7 @@ TEST( GaussianFactor, eliminate ) sigmas(1) = 1/sqrt(125.0); // Check the conditional Gaussian - ConditionalGaussian expectedCG("x2", d,R11,"l1",S12,"x1",S13,sigmas); + GaussianConditional expectedCG("x2", d,R11,"l1",S12,"x1",S13,sigmas); // the expected linear factor double sigma = 0.2236; @@ -403,7 +403,7 @@ TEST( GaussianFactor, eliminate2 ) GaussianFactor combined(meas, b2, sigmas); // eliminate the combined factor - ConditionalGaussian::shared_ptr actualCG; + GaussianConditional::shared_ptr actualCG; GaussianFactor::shared_ptr actualLF; boost::tie(actualCG,actualLF) = combined.eliminate("x2"); @@ -422,7 +422,7 @@ TEST( GaussianFactor, eliminate2 ) x2Sigmas(0) = 0.0894427; x2Sigmas(1) = 0.0894427; - ConditionalGaussian expectedCG("x2",d,R11,"l1x1",S12,x2Sigmas); + GaussianConditional expectedCG("x2",d,R11,"l1x1",S12,x2Sigmas); // the expected linear factor double sigma = 0.2236; @@ -458,12 +458,12 @@ TEST( GaussianFactor, eliminate_empty ) GaussianFactor f; // eliminate the empty factor - ConditionalGaussian::shared_ptr actualCG; + GaussianConditional::shared_ptr actualCG; GaussianFactor::shared_ptr actualLF; boost::tie(actualCG,actualLF) = f.eliminate("x2"); // expected Conditional Gaussian is just a parent-less node with P(x)=1 - ConditionalGaussian expectedCG("x2"); + GaussianConditional expectedCG("x2"); // expected remaining factor is still empty :-) GaussianFactor expectedLF; @@ -612,7 +612,7 @@ TEST( GaussianFactor, size ) } /* ************************************************************************* */ -TEST( GaussianFactor, CONSTRUCTOR_ConditionalGaussian ) +TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional ) { Matrix R11 = Matrix_(2,2, 1.00, 0.00, @@ -628,7 +628,7 @@ TEST( GaussianFactor, CONSTRUCTOR_ConditionalGaussian ) sigmas(0) = 0.29907; sigmas(1) = 0.29907; - ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12,sigmas)); + GaussianConditional::shared_ptr CG(new GaussianConditional("x2",d,R11,"l1x1",S12,sigmas)); GaussianFactor actualLF(CG); // actualLF.print(); GaussianFactor expectedLF("x2",R11,"l1x1",S12,d, sigmas(0)); @@ -645,7 +645,7 @@ TEST ( GaussianFactor, constraint_eliminate1 ) GaussianFactor lc(key, eye(2), v, 0.0); // eliminate it - ConditionalGaussian::shared_ptr actualCG; + GaussianConditional::shared_ptr actualCG; GaussianFactor::shared_ptr actualLF; boost::tie(actualCG,actualLF) = lc.eliminate("x0"); @@ -654,7 +654,7 @@ TEST ( GaussianFactor, constraint_eliminate1 ) // verify conditional Gaussian Vector sigmas = Vector_(2, 0.0, 0.0); - ConditionalGaussian expCG("x0", v, eye(2), sigmas); + GaussianConditional expCG("x0", v, eye(2), sigmas); CHECK(assert_equal(expCG, *actualCG)); } @@ -678,7 +678,7 @@ TEST ( GaussianFactor, constraint_eliminate2 ) GaussianFactor lc("x", A1, "y", A2, b, 0.0); // eliminate x and verify results - ConditionalGaussian::shared_ptr actualCG; + GaussianConditional::shared_ptr actualCG; GaussianFactor::shared_ptr actualLF; boost::tie(actualCG, actualLF) = lc.eliminate("x"); @@ -694,7 +694,7 @@ TEST ( GaussianFactor, constraint_eliminate2 ) 1.0, 2.0, 0.0, 0.0); Vector d = Vector_(2, 3.0, 0.6666); - ConditionalGaussian expectedCG("x", d, R, "y", S, zero(2)); + GaussianConditional expectedCG("x", d, R, "y", S, zero(2)); CHECK(assert_equal(expectedCG, *actualCG, 1e-4)); } @@ -724,7 +724,7 @@ TEST ( GaussianFactor, constraint_eliminate3 ) // eliminate y from original graph // NOTE: this will throw an exception, as // the leading matrix is rank deficient - ConditionalGaussian::shared_ptr actualCG; + GaussianConditional::shared_ptr actualCG; GaussianFactor::shared_ptr actualLF; try { boost::tie(actualCG, actualLF) = lc.eliminate("y"); diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index 846642cb1..c19ba6aa4 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -190,12 +190,12 @@ TEST( GaussianFactorGraph, combine_factors_x2 ) TEST( GaussianFactorGraph, eliminateOne_x1 ) { GaussianFactorGraph fg = createGaussianFactorGraph(); - ConditionalGaussian::shared_ptr actual = fg.eliminateOne("x1"); + GaussianConditional::shared_ptr actual = fg.eliminateOne("x1"); // create expected Conditional Gaussian Matrix I = eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; Vector d = Vector_(2, -0.133333, -0.0222222), sigma = repeat(2, 1./15); - ConditionalGaussian expected("x1",d,R11,"l1",S12,"x2",S13,sigma); + GaussianConditional expected("x1",d,R11,"l1",S12,"x2",S13,sigma); CHECK(assert_equal(expected,*actual,tol)); } @@ -205,12 +205,12 @@ TEST( GaussianFactorGraph, eliminateOne_x1 ) TEST( GaussianFactorGraph, eliminateOne_x2 ) { GaussianFactorGraph fg = createGaussianFactorGraph(); - ConditionalGaussian::shared_ptr actual = fg.eliminateOne("x2"); + GaussianConditional::shared_ptr actual = fg.eliminateOne("x2"); // create expected Conditional Gaussian Matrix I = eye(2), R11 = I, S12 = -0.2*I, S13 = -0.8*I; Vector d = Vector_(2, 0.2, -0.14), sigma = repeat(2, 0.0894427); - ConditionalGaussian expected("x2",d,R11,"l1",S12,"x1",S13,sigma); + GaussianConditional expected("x2",d,R11,"l1",S12,"x1",S13,sigma); CHECK(assert_equal(expected,*actual,tol)); } @@ -219,12 +219,12 @@ TEST( GaussianFactorGraph, eliminateOne_x2 ) TEST( GaussianFactorGraph, eliminateOne_l1 ) { GaussianFactorGraph fg = createGaussianFactorGraph(); - ConditionalGaussian::shared_ptr actual = fg.eliminateOne("l1"); + GaussianConditional::shared_ptr actual = fg.eliminateOne("l1"); // create expected Conditional Gaussian Matrix I = eye(2), R11 = I, S12 = -0.5*I, S13 = -0.5*I; Vector d = Vector_(2, -0.1, 0.25), sigma = repeat(2, 0.141421); - ConditionalGaussian expected("l1",d,R11,"x1",S12,"x2",S13,sigma); + GaussianConditional expected("l1",d,R11,"x1",S12,"x2",S13,sigma); CHECK(assert_equal(expected,*actual,tol)); } @@ -353,7 +353,7 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) // Base FactorGraph only FactorGraph fg3(CBN); - GaussianBayesNet CBN3 = gtsam::eliminate(fg3,ord); + GaussianBayesNet CBN3 = gtsam::eliminate(fg3,ord); CHECK(assert_equal(CBN,CBN3)); } diff --git a/cpp/testInference.cpp b/cpp/testInference.cpp index 744d322c1..862d4de80 100644 --- a/cpp/testInference.cpp +++ b/cpp/testInference.cpp @@ -28,8 +28,8 @@ TEST(GaussianFactorGraph, createSmoother) Ordering ordering; GaussianBayesNet bayesNet = fg2.eliminate(ordering); bayesNet.print("bayesNet"); - FactorGraph p_x3 = marginalize(bayesNet, Ordering("x3")); - FactorGraph p_x1 = marginalize(bayesNet, Ordering("x1")); + FactorGraph p_x3 = marginalize(bayesNet, Ordering("x3")); + FactorGraph p_x1 = marginalize(bayesNet, Ordering("x1")); CHECK(assert_equal(p_x1,p_x3)); // should be the same because of symmetry } @@ -39,10 +39,10 @@ TEST( Inference, marginals ) // create and marginalize a small Bayes net on "x" GaussianBayesNet cbn = createSmallGaussianBayesNet(); Ordering keys("x"); - FactorGraph fg = marginalize(cbn,keys); + FactorGraph fg = marginalize(cbn,keys); // turn into Bayes net to test easily - BayesNet actual = eliminate(fg,keys); + BayesNet actual = eliminate(fg,keys); // expected is just scalar Gaussian on x GaussianBayesNet expected = scalarGaussian("x",4,sqrt(2)); diff --git a/cpp/testNonlinearEquality.cpp b/cpp/testNonlinearEquality.cpp new file mode 100644 index 000000000..241811e3d --- /dev/null +++ b/cpp/testNonlinearEquality.cpp @@ -0,0 +1,16 @@ +/* + * @file testNonlinearEquality.cpp + * @author Alex Cunningham + */ + + +#include +#include "NonlinearEquality.h" + +TEST ( NonlinearEquality, construction ) { + +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/cpp/timeGaussianFactor.cpp b/cpp/timeGaussianFactor.cpp index 1e7e8a93f..ac16901b1 100644 --- a/cpp/timeGaussianFactor.cpp +++ b/cpp/timeGaussianFactor.cpp @@ -69,7 +69,7 @@ int main() GaussianFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2); long timeLog = clock(); int n = 1000000; - ConditionalGaussian::shared_ptr conditional; + GaussianConditional::shared_ptr conditional; GaussianFactor::shared_ptr factor; for(int i = 0; i < n; i++)