diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 58d7595ac..43eae0926 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -143,9 +143,6 @@ namespace gtsam { shared_ptr combine(const Ordering& keys, ADT::Binary op) const; - /** Test whether the factor is empty */ - virtual bool empty() const { return size() == 0; } - // /** // * @brief Permutes the keys in Potentials and DiscreteFactor // * diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index e0c192a13..727214d61 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -97,10 +97,6 @@ Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const { /* ******************************************************************************** */ void DiscreteConditional::solveInPlace(Values& values) const { // TODO: Abhijit asks: is this really the fastest way? He thinks it is. - print("Me: "); - values.print("values:"); - cout << "nrFrontals/nrParents: " << nrFrontals() << "/" << nrParents() << endl; - cout << "first frontal: " << firstFrontalKey() << endl; ADT pFS = choose(values); // P(F|S=parentsValues) // Initialize diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 4201ec62a..b59fe00d6 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -83,7 +83,7 @@ public: } /** Test whether the factor is empty */ - virtual bool empty() const = 0; + virtual bool empty() const { return size() == 0; } /// @} /// @name Standard Interface diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index f2321fa2a..394dc766c 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -3,7 +3,7 @@ set (gtsam_unstable_subdirs base geometry - #discrete + discrete dynamics nonlinear slam @@ -43,7 +43,7 @@ endforeach(subdir) set(gtsam_unstable_srcs ${base_srcs} ${geometry_srcs} - #${discrete_srcs} + ${discrete_srcs} ${dynamics_srcs} ${nonlinear_srcs} ${slam_srcs} diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index def9ef96c..b95a29d05 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -37,6 +37,18 @@ namespace gtsam { virtual void print(const std::string& s = "", const IndexFormatter& formatter = DefaultIndexFormatter) const; + /// equals + bool equals(const DiscreteFactor& other, double tol) const { + if(!dynamic_cast(&other)) + return false; + else { + const AllDiff& f(static_cast(other)); + return cardinalities_.size() == f.cardinalities_.size() + && std::equal(cardinalities_.begin(), cardinalities_.end(), + f.cardinalities_.begin()); + } + } + /// Calculate value = expensive ! virtual double operator()(const Values& values) const; diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index 49867117d..4c75a59a6 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -39,6 +39,16 @@ namespace gtsam { << formatter(keys_[1]) << std::endl; } + /// equals + bool equals(const DiscreteFactor& other, double tol) const { + if(!dynamic_cast(&other)) + return false; + else { + const BinaryAllDiff& f(static_cast(other)); + return (cardinality0_==f.cardinality0_) && (cardinality1_==f.cardinality1_); + } + } + /// Calculate value virtual double operator()(const Values& values) const { return (double) (values.at(keys_[0]) != values.at(keys_[1])); diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index 93fef162c..45164e203 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include @@ -17,15 +16,14 @@ namespace gtsam { /// Find the best total assignment - can be expensive CSP::sharedValues CSP::optimalAssignment() const { - DiscreteSequentialSolver solver(*this); - DiscreteBayesNet::shared_ptr chordal = solver.eliminate(); - sharedValues mpe = optimize(*chordal); + DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(); + sharedValues mpe = chordal->optimize(); return mpe; } void CSP::runArcConsistency(size_t cardinality, size_t nrIterations, bool print) const { // Create VariableIndex - VariableIndexOrdered index(*this); + VariableIndex index(*this); // index.print(); size_t n = index.size(); @@ -46,7 +44,7 @@ namespace gtsam { // keep track of which domains changed changed[v] = false; // loop over all factors/constraints for variable v - const VariableIndexOrdered::Factors& factors = index[v]; + const VariableIndex::Factors& factors = index[v]; BOOST_FOREACH(size_t f,factors) { // if not already a singleton if (!domains[v].isSingleton()) { diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index 561e9a570..76c7385ce 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { @@ -43,12 +44,12 @@ namespace gtsam { /// Construct unary factor Constraint(Index j) : - DiscreteFactor(j) { + DiscreteFactor(boost::assign::cref_list_of<1>(j)) { } /// Construct binary factor Constraint(Index j1, Index j2) : - DiscreteFactor(j1, j2) { + DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) { } /// construct from container diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index c2f793759..33e6a2709 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -69,6 +69,16 @@ namespace gtsam { virtual void print(const std::string& s = "", const IndexFormatter& formatter = DefaultIndexFormatter) const; + /// equals + bool equals(const DiscreteFactor& other, double tol) const { + if(!dynamic_cast(&other)) + return false; + else { + const Domain& f(static_cast(other)); + return (cardinality_==f.cardinality_) && (values_==f.values_); + } + } + bool contains(size_t value) const { return values_.count(value)>0; } diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index cf0e6f199..293f249e7 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include @@ -257,11 +256,8 @@ namespace gtsam { /** Eliminate, return a Bayes net */ DiscreteBayesNet::shared_ptr Scheduler::eliminate() const { - gttic(my_solver); - DiscreteSequentialSolver solver(*this); - gttoc(my_solver); gttic(my_eliminate); - DiscreteBayesNet::shared_ptr chordal = solver.eliminate(); + DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(); gttoc(my_eliminate); return chordal; } @@ -271,14 +267,14 @@ namespace gtsam { DiscreteBayesNet::shared_ptr chordal = eliminate(); if (ISDEBUG("Scheduler::optimalAssignment")) { - DiscreteBayesNet::const_reverse_iterator it = chordal->rbegin(); + DiscreteBayesNet::const_iterator it = chordal->end()-1; const Student & student = students_.front(); cout << endl; (*it)->print(student.name_); } gttic(my_optimize); - sharedValues mpe = optimize(*chordal); + sharedValues mpe = chordal->optimize(); gttoc(my_optimize); return mpe; } diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 574484694..3cb972426 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -45,6 +45,16 @@ namespace gtsam { virtual void print(const std::string& s = "", const IndexFormatter& formatter = DefaultIndexFormatter) const; + /// equals + bool equals(const DiscreteFactor& other, double tol) const { + if(!dynamic_cast(&other)) + return false; + else { + const SingleValue& f(static_cast(other)); + return (cardinality_==f.cardinality_) && (value_==f.value_); + } + } + /// Calculate value virtual double operator()(const Values& values) const; diff --git a/gtsam_unstable/discrete/tests/testCSP.cpp b/gtsam_unstable/discrete/tests/testCSP.cpp index 29cea692c..e987e9642 100644 --- a/gtsam_unstable/discrete/tests/testCSP.cpp +++ b/gtsam_unstable/discrete/tests/testCSP.cpp @@ -123,6 +123,11 @@ TEST_UNSAFE( CSP, WesternUS) (WA.first,1)(CA.first,1)(NV.first,3)(OR.first,0) (MT.first,1)(WY.first,0)(NM.first,3)(CO.first,2) (ID.first,2)(UT.first,1)(AZ.first,0); + + // TODO: Fix me! mpe result seems to be right. (See the printing) + // It has the same prob as the expected solution. + // Is mpe another solution, or the expected solution is unique??? + cout << csp(*mpe) << " should be >= " << csp(expected) << endl; EXPECT(assert_equal(expected,*mpe)); EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp index 406ca0c8e..fa68f04ea 100644 --- a/gtsam_unstable/discrete/tests/testScheduler.cpp +++ b/gtsam_unstable/discrete/tests/testScheduler.cpp @@ -74,7 +74,7 @@ DiscreteFactorGraph createExpected() { expected.addAllDiff(J1 & J2 & J3); // Mutual exclusion for students - expected.addAllDiff(A & J); + expected.addAllDiff(A, J); return expected; }