diff --git a/.gitignore b/.gitignore index cde059767..e6e38132f 100644 --- a/.gitignore +++ b/.gitignore @@ -3,6 +3,7 @@ .idea *.pyc *.DS_Store +*.swp /examples/Data/dubrovnik-3-7-pre-rewritten.txt /examples/Data/pose2example-rewritten.txt /examples/Data/pose3example-rewritten.txt diff --git a/examples/DiscreteBayesNetExample.cpp b/examples/DiscreteBayesNetExample.cpp index 5dca116c3..febc1e128 100644 --- a/examples/DiscreteBayesNetExample.cpp +++ b/examples/DiscreteBayesNetExample.cpp @@ -56,8 +56,8 @@ int main(int argc, char **argv) { DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering); // solve - DiscreteFactor::sharedValues mpe = chordal->optimize(); - GTSAM_PRINT(*mpe); + auto mpe = chordal->optimize(); + GTSAM_PRINT(mpe); // We can also build a Bayes tree (directed junction tree). // The elimination order above will do fine: @@ -70,14 +70,14 @@ int main(int argc, char **argv) { // solve again, now with evidence DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering); - DiscreteFactor::sharedValues mpe2 = chordal2->optimize(); - GTSAM_PRINT(*mpe2); + auto mpe2 = chordal2->optimize(); + GTSAM_PRINT(mpe2); // We can also sample from it cout << "\n10 samples:" << endl; for (size_t i = 0; i < 10; i++) { - DiscreteFactor::sharedValues sample = chordal2->sample(); - GTSAM_PRINT(*sample); + auto sample = chordal2->sample(); + GTSAM_PRINT(sample); } return 0; } diff --git a/examples/DiscreteBayesNet_FG.cpp b/examples/DiscreteBayesNet_FG.cpp index 121df4bef..69283a1be 100644 --- a/examples/DiscreteBayesNet_FG.cpp +++ b/examples/DiscreteBayesNet_FG.cpp @@ -33,11 +33,11 @@ using namespace gtsam; int main(int argc, char **argv) { // Define keys and a print function Key C(1), S(2), R(3), W(4); - auto print = [=](DiscreteFactor::sharedValues values) { - cout << boolalpha << "Cloudy = " << static_cast((*values)[C]) - << " Sprinkler = " << static_cast((*values)[S]) - << " Rain = " << boolalpha << static_cast((*values)[R]) - << " WetGrass = " << static_cast((*values)[W]) << endl; + auto print = [=](const DiscreteFactor::Values& values) { + cout << boolalpha << "Cloudy = " << static_cast(values.at(C)) + << " Sprinkler = " << static_cast(values.at(S)) + << " Rain = " << boolalpha << static_cast(values.at(R)) + << " WetGrass = " << static_cast(values.at(W)) << endl; }; // We assume binary state variables @@ -85,7 +85,7 @@ int main(int argc, char **argv) { } // "Most Probable Explanation", i.e., configuration with largest value - DiscreteFactor::sharedValues mpe = graph.eliminateSequential()->optimize(); + auto mpe = graph.eliminateSequential()->optimize(); cout << "\nMost Probable Explanation (MPE):" << endl; print(mpe); @@ -97,7 +97,7 @@ int main(int argc, char **argv) { // solve again, now with evidence DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); - DiscreteFactor::sharedValues mpe_with_evidence = chordal->optimize(); + auto mpe_with_evidence = chordal->optimize(); cout << "\nMPE given C=0:" << endl; print(mpe_with_evidence); @@ -113,7 +113,7 @@ int main(int argc, char **argv) { // We can also sample from it cout << "\n10 samples:" << endl; for (size_t i = 0; i < 10; i++) { - DiscreteFactor::sharedValues sample = chordal->sample(); + auto sample = chordal->sample(); print(sample); } return 0; diff --git a/examples/HMMExample.cpp b/examples/HMMExample.cpp index ee861e381..b46baf4e0 100644 --- a/examples/HMMExample.cpp +++ b/examples/HMMExample.cpp @@ -66,14 +66,14 @@ int main(int argc, char **argv) { chordal->print("Eliminated"); // solve - DiscreteFactor::sharedValues mpe = chordal->optimize(); - GTSAM_PRINT(*mpe); + auto mpe = chordal->optimize(); + GTSAM_PRINT(mpe); // We can also sample from it cout << "\n10 samples:" << endl; for (size_t k = 0; k < 10; k++) { - DiscreteFactor::sharedValues sample = chordal->sample(); - GTSAM_PRINT(*sample); + auto sample = chordal->sample(); + GTSAM_PRINT(sample); } // Or compute the marginals. This re-eliminates the FG into a Bayes tree diff --git a/examples/UGM_chain.cpp b/examples/UGM_chain.cpp index 3a885a844..ababef022 100644 --- a/examples/UGM_chain.cpp +++ b/examples/UGM_chain.cpp @@ -70,8 +70,8 @@ int main(int argc, char** argv) { // "Decoding", i.e., configuration with largest value // We use sequential variable elimination DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); - DiscreteFactor::sharedValues optimalDecoding = chordal->optimize(); - optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n"); + auto optimalDecoding = chordal->optimize(); + optimalDecoding.print("\nMost Probable Explanation (optimalDecoding)\n"); // "Inference" Computing marginals for each node // Here we'll make use of DiscreteMarginals class, which makes use of diff --git a/examples/UGM_small.cpp b/examples/UGM_small.cpp index 27a6205a3..f4f3f1fd0 100644 --- a/examples/UGM_small.cpp +++ b/examples/UGM_small.cpp @@ -63,8 +63,8 @@ int main(int argc, char** argv) { // "Decoding", i.e., configuration with largest value (MPE) // We use sequential variable elimination DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); - DiscreteFactor::sharedValues optimalDecoding = chordal->optimize(); - optimalDecoding->print("\noptimalDecoding"); + auto optimalDecoding = chordal->optimize(); + GTSAM_PRINT(optimalDecoding); // "Inference" Computing marginals cout << "\nComputing Node Marginals .." << endl; diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index ac7c2a9a5..cb8e7d017 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -370,4 +370,4 @@ public: * the gtsam namespace to be more easily enforced as testable */ #define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup; -#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::IsLieGroup _gtsam_IsLieGroup_##T; +#define GTSAM_CONCEPT_LIE_TYPE(T) using _gtsam_IsLieGroup_##T = gtsam::IsLieGroup; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index dbe497005..962dc8269 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -178,4 +178,4 @@ struct FixedDimension { // * the gtsam namespace to be more easily enforced as testable // */ #define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold; -#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::IsManifold _gtsam_IsManifold_##T; +#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold; diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 013947bbd..61c61a5af 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -46,28 +46,28 @@ typedef Eigen::Matrix M // Create handy typedefs and constants for square-size matrices // MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9 #define GTSAM_MAKE_MATRIX_DEFS(N) \ -typedef Eigen::Matrix Matrix##N; \ -typedef Eigen::Matrix Matrix1##N; \ -typedef Eigen::Matrix Matrix2##N; \ -typedef Eigen::Matrix Matrix3##N; \ -typedef Eigen::Matrix Matrix4##N; \ -typedef Eigen::Matrix Matrix5##N; \ -typedef Eigen::Matrix Matrix6##N; \ -typedef Eigen::Matrix Matrix7##N; \ -typedef Eigen::Matrix Matrix8##N; \ -typedef Eigen::Matrix Matrix9##N; \ +using Matrix##N = Eigen::Matrix; \ +using Matrix1##N = Eigen::Matrix; \ +using Matrix2##N = Eigen::Matrix; \ +using Matrix3##N = Eigen::Matrix; \ +using Matrix4##N = Eigen::Matrix; \ +using Matrix5##N = Eigen::Matrix; \ +using Matrix6##N = Eigen::Matrix; \ +using Matrix7##N = Eigen::Matrix; \ +using Matrix8##N = Eigen::Matrix; \ +using Matrix9##N = Eigen::Matrix; \ static const Eigen::MatrixBase::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \ static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = Matrix##N::Zero(); -GTSAM_MAKE_MATRIX_DEFS(1); -GTSAM_MAKE_MATRIX_DEFS(2); -GTSAM_MAKE_MATRIX_DEFS(3); -GTSAM_MAKE_MATRIX_DEFS(4); -GTSAM_MAKE_MATRIX_DEFS(5); -GTSAM_MAKE_MATRIX_DEFS(6); -GTSAM_MAKE_MATRIX_DEFS(7); -GTSAM_MAKE_MATRIX_DEFS(8); -GTSAM_MAKE_MATRIX_DEFS(9); +GTSAM_MAKE_MATRIX_DEFS(1) +GTSAM_MAKE_MATRIX_DEFS(2) +GTSAM_MAKE_MATRIX_DEFS(3) +GTSAM_MAKE_MATRIX_DEFS(4) +GTSAM_MAKE_MATRIX_DEFS(5) +GTSAM_MAKE_MATRIX_DEFS(6) +GTSAM_MAKE_MATRIX_DEFS(7) +GTSAM_MAKE_MATRIX_DEFS(8) +GTSAM_MAKE_MATRIX_DEFS(9) // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 6062c7ae1..d50d62c1f 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -173,4 +173,4 @@ namespace gtsam { * @deprecated please use BOOST_CONCEPT_ASSERT and */ #define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable; -#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::IsTestable _gtsam_Testable_##T; +#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable; diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 0e6e1c276..c86fbb6d2 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -85,7 +85,7 @@ bool assert_equal(const V& expected, const boost::optional& actual, do * \deprecated: use container equals instead */ template -bool assert_equal(const std::vector& expected, const std::vector& actual, double tol = 1e-9) { +bool GTSAM_DEPRECATED assert_equal(const std::vector& expected, const std::vector& actual, double tol = 1e-9) { bool match = true; if (expected.size() != actual.size()) match = false; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index ed90a7126..a057da46b 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -48,18 +48,18 @@ static const Eigen::MatrixBase::ConstantReturnType Z_3x1 = Vector3::Zer // Create handy typedefs and constants for vectors with N>3 // VectorN and Z_Nx1, for N=1..9 #define GTSAM_MAKE_VECTOR_DEFS(N) \ - typedef Eigen::Matrix Vector##N; \ + using Vector##N = Eigen::Matrix; \ static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = Vector##N::Zero(); -GTSAM_MAKE_VECTOR_DEFS(4); -GTSAM_MAKE_VECTOR_DEFS(5); -GTSAM_MAKE_VECTOR_DEFS(6); -GTSAM_MAKE_VECTOR_DEFS(7); -GTSAM_MAKE_VECTOR_DEFS(8); -GTSAM_MAKE_VECTOR_DEFS(9); -GTSAM_MAKE_VECTOR_DEFS(10); -GTSAM_MAKE_VECTOR_DEFS(11); -GTSAM_MAKE_VECTOR_DEFS(12); +GTSAM_MAKE_VECTOR_DEFS(4) +GTSAM_MAKE_VECTOR_DEFS(5) +GTSAM_MAKE_VECTOR_DEFS(6) +GTSAM_MAKE_VECTOR_DEFS(7) +GTSAM_MAKE_VECTOR_DEFS(8) +GTSAM_MAKE_VECTOR_DEFS(9) +GTSAM_MAKE_VECTOR_DEFS(10) +GTSAM_MAKE_VECTOR_DEFS(11) +GTSAM_MAKE_VECTOR_DEFS(12) typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; @@ -207,14 +207,14 @@ inline double inner_prod(const V1 &a, const V2& b) { * BLAS Level 1 scal: x <- alpha*x * \deprecated: use operators instead */ -inline void scal(double alpha, Vector& x) { x *= alpha; } +inline void GTSAM_DEPRECATED scal(double alpha, Vector& x) { x *= alpha; } /** * BLAS Level 1 axpy: y <- alpha*x + y * \deprecated: use operators instead */ template -inline void axpy(double alpha, const V1& x, V2& y) { +inline void GTSAM_DEPRECATED axpy(double alpha, const V1& x, V2& y) { assert (y.size()==x.size()); y += alpha * x; } diff --git a/gtsam/base/chartTesting.h b/gtsam/base/chartTesting.h index f63054a5b..8f5213f91 100644 --- a/gtsam/base/chartTesting.h +++ b/gtsam/base/chartTesting.h @@ -32,7 +32,7 @@ void testDefaultChart(TestResult& result_, const std::string& name_, const T& value) { - GTSAM_CONCEPT_TESTABLE_TYPE(T); + GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef typename gtsam::DefaultChart Chart; typedef typename Chart::vector Vector; diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index f589ecc5e..24355c684 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -19,8 +19,9 @@ #pragma once -#include +#include #include +#include #include // includes for standard serialization types @@ -40,6 +41,17 @@ #include #include +// Workaround a bug in GCC >= 7 and C++17 +// ref. https://gitlab.com/libeigen/eigen/-/issues/1676 +#ifdef __GNUC__ +#if __GNUC__ >= 7 && __cplusplus >= 201703L +namespace boost { namespace serialization { struct U; } } +namespace Eigen { namespace internal { +template<> struct traits {enum {Flags=0};}; +} } +#endif +#endif + namespace gtsam { /** @name Standard serialization diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index bd715e3cb..c87732b09 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -220,8 +220,8 @@ TEST(Vector, axpy ) Vector x = Vector3(10., 20., 30.); Vector y0 = Vector3(2.0, 5.0, 6.0); Vector y1 = y0, y2 = y0; - axpy(0.1,x,y1); - axpy(0.1,x,y2.head(3)); + y1 += 0.1 * x; + y2.head(3) += 0.1 * x; Vector expected = Vector3(3.0, 7.0, 9.0); EXPECT(assert_equal(expected,y1)); EXPECT(assert_equal(expected,Vector(y2))); diff --git a/gtsam/base/types.h b/gtsam/base/types.h index aaada3cee..a0d24f1a6 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -34,6 +34,14 @@ #include #endif +#if defined(__GNUC__) || defined(__clang__) +#define GTSAM_DEPRECATED __attribute__((deprecated)) +#elif defined(_MSC_VER) +#define GTSAM_DEPRECATED __declspec(deprecated) +#else +#define GTSAM_DEPRECATED +#endif + #ifdef GTSAM_USE_EIGEN_MKL_OPENMP #include #endif diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index 84a80c565..71c50c477 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -54,20 +54,20 @@ namespace gtsam { } /* ************************************************************************* */ - DiscreteFactor::sharedValues DiscreteBayesNet::optimize() const { + DiscreteFactor::Values DiscreteBayesNet::optimize() const { // solve each node in turn in topological sort order (parents first) - DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); + DiscreteFactor::Values result; for (auto conditional: boost::adaptors::reverse(*this)) - conditional->solveInPlace(*result); + conditional->solveInPlace(&result); return result; } /* ************************************************************************* */ - DiscreteFactor::sharedValues DiscreteBayesNet::sample() const { + DiscreteFactor::Values DiscreteBayesNet::sample() const { // sample each node in turn in topological sort order (parents first) - DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); + DiscreteFactor::Values result; for (auto conditional: boost::adaptors::reverse(*this)) - conditional->sampleInPlace(*result); + conditional->sampleInPlace(&result); return result; } diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index d5ba30584..e89645658 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -83,10 +83,10 @@ namespace gtsam { /** * Solve the DiscreteBayesNet by back-substitution */ - DiscreteFactor::sharedValues optimize() const; + DiscreteFactor::Values optimize() const; /** Do ancestral sampling */ - DiscreteFactor::sharedValues sample() const; + DiscreteFactor::Values sample() const; ///@} diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index ac7c58405..26a43a10c 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -35,7 +35,7 @@ using namespace std; namespace gtsam { // Instantiate base class -template class Conditional ; +template class GTSAM_EXPORT Conditional ; /* ******************************************************************************** */ DiscreteConditional::DiscreteConditional(const size_t nrFrontals, @@ -117,9 +117,9 @@ Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const { } /* ******************************************************************************** */ -void DiscreteConditional::solveInPlace(Values& values) const { +void DiscreteConditional::solveInPlace(Values* values) const { // TODO: Abhijit asks: is this really the fastest way? He thinks it is. - ADT pFS = choose(values); // P(F|S=parentsValues) + ADT pFS = choose(*values); // P(F|S=parentsValues) // Initialize Values mpe; @@ -145,16 +145,16 @@ void DiscreteConditional::solveInPlace(Values& values) const { //set values (inPlace) to mpe for(Key j: frontals()) { - values[j] = mpe[j]; + (*values)[j] = mpe[j]; } } /* ******************************************************************************** */ -void DiscreteConditional::sampleInPlace(Values& values) const { +void DiscreteConditional::sampleInPlace(Values* values) const { assert(nrFrontals() == 1); Key j = (firstFrontalKey()); - size_t sampled = sample(values); // Sample variable - values[j] = sampled; // store result in partial solution + size_t sampled = sample(*values); // Sample variable given parents + (*values)[j] = sampled; // store result in partial solution } /* ******************************************************************************** */ diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 8299fab2c..191a80fb0 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -45,7 +45,6 @@ public: /** A map from keys to values.. * TODO: Again, do we need this??? */ typedef Assignment Values; - typedef boost::shared_ptr sharedValues; /// @name Standard Constructors /// @{ @@ -133,10 +132,10 @@ public: /// @{ /// solve a conditional, in place - void solveInPlace(Values& parentsValues) const; + void solveInPlace(Values* parentsValues) const; /// sample in place, stores result in partial solution - void sampleInPlace(Values& parentsValues) const; + void sampleInPlace(Values* parentsValues) const; /// @} diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 6b0919507..cd883c59c 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -51,7 +51,6 @@ public: * the new class DiscreteValue, as the varible's type (domain) */ typedef Assignment Values; - typedef boost::shared_ptr sharedValues; public: diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index e41968d6b..4ff0e339e 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -94,7 +94,7 @@ namespace gtsam { // } /* ************************************************************************* */ - DiscreteFactor::sharedValues DiscreteFactorGraph::optimize() const + DiscreteFactorGraph::Values DiscreteFactorGraph::optimize() const { gttic(DiscreteFactorGraph_optimize); return BaseEliminateable::eliminateSequential()->optimize(); diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index f39adc9a8..329d015e9 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -74,7 +74,6 @@ public: /** A map from keys to values */ typedef KeyVector Indices; typedef Assignment Values; - typedef boost::shared_ptr sharedValues; /** Default constructor */ DiscreteFactorGraph() {} @@ -101,25 +100,27 @@ public: /// @} - template + // Add single key decision-tree factor. + template void add(const DiscreteKey& j, SOURCE table) { DiscreteKeys keys; keys.push_back(j); - push_back(boost::make_shared(keys, table)); + emplace_shared(keys, table); } - template + // Add binary key decision-tree factor. + template void add(const DiscreteKey& j1, const DiscreteKey& j2, SOURCE table) { DiscreteKeys keys; keys.push_back(j1); keys.push_back(j2); - push_back(boost::make_shared(keys, table)); + emplace_shared(keys, table); } - /** add shared discreteFactor immediately from arguments */ - template + // Add shared discreteFactor immediately from arguments. + template void add(const DiscreteKeys& keys, SOURCE table) { - push_back(boost::make_shared(keys, table)); + emplace_shared(keys, table); } /** Return the set of variables involved in the factors (set union) */ @@ -140,7 +141,7 @@ public: * the dense elimination function specified in \c function, * followed by back-substitution resulting from elimination. Is equivalent * to calling graph.eliminateSequential()->optimize(). */ - DiscreteFactor::sharedValues optimize() const; + Values optimize() const; // /** Permute the variables in the factors */ diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index 2b440e5a0..f6159c0c6 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -104,12 +104,12 @@ TEST(DiscreteBayesNet, Asia) { EXPECT(assert_equal(expected2, *chordal->back())); // solve - DiscreteFactor::sharedValues actualMPE = chordal->optimize(); + auto actualMPE = chordal->optimize(); DiscreteFactor::Values expectedMPE; insert(expectedMPE)(Asia.first, 0)(Dyspnea.first, 0)(XRay.first, 0)( Tuberculosis.first, 0)(Smoking.first, 0)(Either.first, 0)( LungCancer.first, 0)(Bronchitis.first, 0); - EXPECT(assert_equal(expectedMPE, *actualMPE)); + EXPECT(assert_equal(expectedMPE, actualMPE)); // add evidence, we were in Asia and we have dyspnea fg.add(Asia, "0 1"); @@ -117,12 +117,12 @@ TEST(DiscreteBayesNet, Asia) { // solve again, now with evidence DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering); - DiscreteFactor::sharedValues actualMPE2 = chordal2->optimize(); + auto actualMPE2 = chordal2->optimize(); DiscreteFactor::Values expectedMPE2; insert(expectedMPE2)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 0)( Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 0)( LungCancer.first, 0)(Bronchitis.first, 1); - EXPECT(assert_equal(expectedMPE2, *actualMPE2)); + EXPECT(assert_equal(expectedMPE2, actualMPE2)); // now sample from it DiscreteFactor::Values expectedSample; @@ -130,8 +130,8 @@ TEST(DiscreteBayesNet, Asia) { insert(expectedSample)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 1)( Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 1)( LungCancer.first, 1)(Bronchitis.first, 0); - DiscreteFactor::sharedValues actualSample = chordal2->sample(); - EXPECT(assert_equal(expectedSample, *actualSample)); + auto actualSample = chordal2->sample(); + EXPECT(assert_equal(expectedSample, actualSample)); } /* ************************************************************************* */ diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 1defd5acf..6b7a43c1c 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -169,8 +169,8 @@ TEST( DiscreteFactorGraph, test) // Test optimization DiscreteFactor::Values expectedValues; insert(expectedValues)(0, 0)(1, 0)(2, 0); - DiscreteFactor::sharedValues actualValues = graph.optimize(); - EXPECT(assert_equal(expectedValues, *actualValues)); + auto actualValues = graph.optimize(); + EXPECT(assert_equal(expectedValues, actualValues)); } /* ************************************************************************* */ @@ -186,11 +186,11 @@ TEST( DiscreteFactorGraph, testMPE) // graph.product().print(); // DiscreteSequentialSolver(graph).eliminate()->print(); - DiscreteFactor::sharedValues actualMPE = graph.optimize(); + auto actualMPE = graph.optimize(); DiscreteFactor::Values expectedMPE; insert(expectedMPE)(0, 0)(1, 1)(2, 1); - EXPECT(assert_equal(expectedMPE, *actualMPE)); + EXPECT(assert_equal(expectedMPE, actualMPE)); } /* ************************************************************************* */ @@ -216,8 +216,8 @@ TEST( DiscreteFactorGraph, testMPE_Darwiche09book_p244) // Use the solver machinery. DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); - DiscreteFactor::sharedValues actualMPE = chordal->optimize(); - EXPECT(assert_equal(expectedMPE, *actualMPE)); + auto actualMPE = chordal->optimize(); + EXPECT(assert_equal(expectedMPE, actualMPE)); // DiscreteConditional::shared_ptr root = chordal->back(); // EXPECT_DOUBLES_EQUAL(0.4, (*root)(*actualMPE), 1e-9); @@ -244,8 +244,8 @@ ETree::shared_ptr eTree = ETree::Create(graph, structure); // eliminate normally and check solution DiscreteBayesNet::shared_ptr bayesNet = eTree->eliminate(&EliminateDiscrete); // bayesNet->print(">>>>>>>>>>>>>> Bayes Net <<<<<<<<<<<<<<<<<<"); -DiscreteFactor::sharedValues actualMPE = optimize(*bayesNet); -EXPECT(assert_equal(expectedMPE, *actualMPE)); +auto actualMPE = optimize(*bayesNet); +EXPECT(assert_equal(expectedMPE, actualMPE)); // Approximate and check solution // DiscreteBayesNet::shared_ptr approximateNet = eTree->approximate(); diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index 52d475d5d..fd2c7ab65 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -46,9 +46,9 @@ double Cal3Fisheye::Scaling(double r) { /* ************************************************************************* */ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, OptionalJacobian<2, 2> H2) const { - const double xi = p.x(), yi = p.y(); + const double xi = p.x(), yi = p.y(), zi = 1; const double r2 = xi * xi + yi * yi, r = sqrt(r2); - const double t = atan(r); + const double t = atan2(r, zi); const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4; Vector5 K, T; K << 1, k1_, k2_, k3_, k4_; @@ -76,28 +76,32 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, // Derivative for points in intrinsic coords (2 by 2) if (H2) { - const double dtd_dt = - 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; - const double dt_dr = 1 / (1 + r2); - const double rinv = 1 / r; - const double dr_dxi = xi * rinv; - const double dr_dyi = yi * rinv; - const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; - const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; + if (r2==0) { + *H2 = DK; + } else { + const double dtd_dt = + 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; + const double R2 = r2 + zi*zi; + const double dt_dr = zi / R2; + const double rinv = 1 / r; + const double dr_dxi = xi * rinv; + const double dr_dyi = yi * rinv; + const double dtd_dr = dtd_dt * dt_dr; + + const double c2 = dr_dxi * dr_dxi; + const double s2 = dr_dyi * dr_dyi; + const double cs = dr_dxi * dr_dyi; - const double td = t * K.dot(T); - const double rrinv = 1 / r2; - const double dxd_dxi = - dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi; - const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi; - const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi; - const double dyd_dyi = - dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi; + const double dxd_dxi = dtd_dr * c2 + s * (1 - c2); + const double dxd_dyi = (dtd_dr - s) * cs; + const double dyd_dxi = dxd_dyi; + const double dyd_dyi = dtd_dr * s2 + s * (1 - s2); - Matrix2 DR; - DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; + Matrix2 DR; + DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; - *H2 = DK * DR; + *H2 = DK * DR; + } } return uv; diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 5442ded84..f364828a1 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -35,7 +35,7 @@ class GTSAM_EXPORT PinholeBaseK: public PinholeBase { private: - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) // Get dimensions of calibration type at compile time static const int DimK = FixedDimension::value; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 8dafffee8..a0b3d502e 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -28,7 +28,7 @@ using namespace std; namespace gtsam { /** instantiate concept checks */ -GTSAM_CONCEPT_POSE_INST(Pose2); +GTSAM_CONCEPT_POSE_INST(Pose2) static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index d0e60f3fd..1e399ad18 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -30,7 +30,7 @@ using std::vector; using Point3Pairs = vector; /** instantiate concept checks */ -GTSAM_CONCEPT_POSE_INST(Pose3); +GTSAM_CONCEPT_POSE_INST(Pose3) /* ************************************************************************* */ Pose3::Pose3(const Pose2& pose2) : @@ -115,7 +115,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) { /* ************************************************************************* */ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi) { + OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { if (Hxi) { Hxi->setZero(); for (int i = 0; i < 6; ++i) { @@ -126,12 +126,14 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, Hxi->col(i) = Gi * y; } } - return adjointMap(xi) * y; + const Matrix6& ad_xi = adjointMap(xi); + if (H_y) *H_y = ad_xi; + return ad_xi * y; } /* ************************************************************************* */ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi) { + OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { if (Hxi) { Hxi->setZero(); for (int i = 0; i < 6; ++i) { @@ -142,7 +144,9 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, Hxi->col(i) = GTi * y; } } - return adjointMap(xi).transpose() * y; + const Matrix6& adT_xi = adjointMap(xi).transpose(); + if (H_y) *H_y = adT_xi; + return adT_xi * y; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index b6107120e..884d0760c 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -177,13 +177,14 @@ public: * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. * */ - static Matrix6 adjointMap(const Vector6 &xi); + static Matrix6 adjointMap(const Vector6& xi); /** * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives */ - static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, - OptionalJacobian<6, 6> Hxi = boost::none); + static Vector6 adjoint(const Vector6& xi, const Vector6& y, + OptionalJacobian<6, 6> Hxi = boost::none, + OptionalJacobian<6, 6> H_y = boost::none); // temporary fix for wrappers until case issue is resolved static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} @@ -193,7 +194,8 @@ public: * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi = boost::none); + OptionalJacobian<6, 6> Hxi = boost::none, + OptionalJacobian<6, 6> H_y = boost::none); /// Derivative of Expmap static Matrix6 ExpmapDerivative(const Vector6& xi); diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 1557a09db..2ef47d58e 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -117,13 +117,23 @@ struct traits { omega = (-8. / 3. - 2. / 3. * qw) * q.vec(); } else { // Normal, away from zero case - _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw); - // Important: convert to [-pi,pi] to keep error continuous - if (angle > M_PI) - angle -= twoPi; - else if (angle < -M_PI) - angle += twoPi; - omega = (angle / s) * q.vec(); + if (qw > 0) { + _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw); + // Important: convert to [-pi,pi] to keep error continuous + if (angle > M_PI) + angle -= twoPi; + else if (angle < -M_PI) + angle += twoPi; + omega = (angle / s) * q.vec(); + } else { + // Make sure that we are using a canonical quaternion with w > 0 + _Scalar angle = 2 * acos(-qw), s = sqrt(1 - qw * qw); + if (angle > M_PI) + angle -= twoPi; + else if (angle < -M_PI) + angle += twoPi; + omega = (angle / s) * -q.vec(); + } } if(H) *H = SO3::LogmapDerivative(omega.template cast()); diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp index fcaf0c874..ea7a6d049 100644 --- a/gtsam/geometry/Similarity3.cpp +++ b/gtsam/geometry/Similarity3.cpp @@ -40,8 +40,8 @@ static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, } /// Form inner products x and y and calculate scale. -static const double calculateScale(const Point3Pairs &d_abPointPairs, - const Rot3 &aRb) { +static double calculateScale(const Point3Pairs &d_abPointPairs, + const Rot3 &aRb) { double x = 0, y = 0; Point3 da, db; for (const Point3Pair& d_abPair : d_abPointPairs) { diff --git a/gtsam/geometry/concepts.h b/gtsam/geometry/concepts.h index 207b48f56..bafb62418 100644 --- a/gtsam/geometry/concepts.h +++ b/gtsam/geometry/concepts.h @@ -72,5 +72,5 @@ private: /** Pose Concept macros */ #define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept; -#define GTSAM_CONCEPT_POSE_TYPE(T) typedef gtsam::PoseConcept _gtsam_PoseConcept##T; +#define GTSAM_CONCEPT_POSE_TYPE(T) using _gtsam_PoseConcept##T = gtsam::PoseConcept; diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 9baa49e8e..a40951d3e 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -473,6 +473,9 @@ class Pose3 { Vector logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; + Vector AdjointTranspose(Vector xi) const; + static Matrix adjointMap(Vector xi); + static Vector adjoint(Vector xi, Vector y); static Matrix adjointMap_(Vector xi); static Vector adjoint_(Vector xi, Vector y); static Vector adjointTranspose(Vector xi, Vector y); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index f0f2c0ccd..e1d3d5ea2 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -912,16 +912,20 @@ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) { } TEST( Pose3, adjoint) { - Vector expected = testDerivAdjoint(screwPose3::xi, screwPose3::xi); + Vector6 v = (Vector6() << 1, 2, 3, 4, 5, 6).finished(); + Vector expected = testDerivAdjoint(screwPose3::xi, v); - Matrix actualH; - Vector actual = Pose3::adjoint(screwPose3::xi, screwPose3::xi, actualH); + Matrix actualH1, actualH2; + Vector actual = Pose3::adjoint(screwPose3::xi, v, actualH1, actualH2); - Matrix numericalH = numericalDerivative21( - testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5); + Matrix numericalH1 = numericalDerivative21( + testDerivAdjoint, screwPose3::xi, v, 1e-5); + Matrix numericalH2 = numericalDerivative22( + testDerivAdjoint, screwPose3::xi, v, 1e-5); EXPECT(assert_equal(expected,actual,1e-5)); - EXPECT(assert_equal(numericalH,actualH,1e-5)); + EXPECT(assert_equal(numericalH1,actualH1,1e-5)); + EXPECT(assert_equal(numericalH2,actualH2,1e-5)); } /* ************************************************************************* */ @@ -934,14 +938,17 @@ TEST( Pose3, adjointTranspose) { Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0).finished(); Vector expected = testDerivAdjointTranspose(xi, v); - Matrix actualH; - Vector actual = Pose3::adjointTranspose(xi, v, actualH); + Matrix actualH1, actualH2; + Vector actual = Pose3::adjointTranspose(xi, v, actualH1, actualH2); - Matrix numericalH = numericalDerivative21( + Matrix numericalH1 = numericalDerivative21( + testDerivAdjointTranspose, xi, v, 1e-5); + Matrix numericalH2 = numericalDerivative22( testDerivAdjointTranspose, xi, v, 1e-5); EXPECT(assert_equal(expected,actual,1e-15)); - EXPECT(assert_equal(numericalH,actualH,1e-5)); + EXPECT(assert_equal(numericalH1,actualH1,1e-5)); + EXPECT(assert_equal(numericalH2,actualH2,1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 7199da0ad..cc003d8dc 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -19,6 +19,8 @@ #pragma once +#include + #include #include #include diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index e225bac5f..7dd414193 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -110,7 +110,7 @@ class ClusterTree { typedef sharedCluster sharedNode; /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType) protected: FastVector roots_; diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 81f4047a1..35e7505c9 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -36,17 +36,17 @@ namespace gtsam { // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex // before creating ordering. VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(function, computedVariableIndex, orderingType); + return eliminateSequential(orderingType, function, computedVariableIndex); } else { // Compute an ordering and call this function again. We are guaranteed to have a // VariableIndex already here because we computed one if needed in the previous 'if' block. if (orderingType == Ordering::METIS) { Ordering computedOrdering = Ordering::Metis(asDerived()); - return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + return eliminateSequential(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = Ordering::Colamd(*variableIndex); - return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + return eliminateSequential(computedOrdering, function, variableIndex); } } } @@ -78,29 +78,31 @@ namespace gtsam { } /* ************************************************************************* */ - template - boost::shared_ptr::BayesTreeType> - EliminateableFactorGraph::eliminateMultifrontal( - OptionalOrderingType orderingType, const Eliminate& function, - OptionalVariableIndex variableIndex) const - { - if(!variableIndex) { - // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check - // for no variable index first so that it's always computed if we need to call COLAMD because - // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex - // before creating ordering. + template + boost::shared_ptr< + typename EliminateableFactorGraph::BayesTreeType> + EliminateableFactorGraph::eliminateMultifrontal( + OptionalOrderingType orderingType, const Eliminate& function, + OptionalVariableIndex variableIndex) const { + if (!variableIndex) { + // If no VariableIndex provided, compute one and call this function again + // IMPORTANT: we check for no variable index first so that it's always + // computed if we need to call COLAMD because no Ordering is provided. + // When removing optional from VariableIndex, create VariableIndex before + // creating ordering. VariableIndex computedVariableIndex(asDerived()); - return eliminateMultifrontal(function, computedVariableIndex, orderingType); - } - else { - // Compute an ordering and call this function again. We are guaranteed to have a - // VariableIndex already here because we computed one if needed in the previous 'if' block. + return eliminateMultifrontal(orderingType, function, + computedVariableIndex); + } else { + // Compute an ordering and call this function again. We are guaranteed to + // have a VariableIndex already here because we computed one if needed in + // the previous 'if' block. if (orderingType == Ordering::METIS) { Ordering computedOrdering = Ordering::Metis(asDerived()); - return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + return eliminateMultifrontal(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = Ordering::Colamd(*variableIndex); - return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + return eliminateMultifrontal(computedOrdering, function, variableIndex); } } } @@ -273,7 +275,7 @@ namespace gtsam { else { // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateSequential(function); + return factorGraph->eliminateSequential(Ordering::COLAMD, function); } } } @@ -340,7 +342,7 @@ namespace gtsam { else { // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateMultifrontal(function); + return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function); } } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index edc4883e7..579eed7f9 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -288,8 +288,9 @@ namespace gtsam { FactorGraphType& asDerived() { return static_cast(*this); } public: + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** \deprecated ordering and orderingType shouldn't both be specified */ - boost::shared_ptr eliminateSequential( + boost::shared_ptr GTSAM_DEPRECATED eliminateSequential( const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex, @@ -298,7 +299,7 @@ namespace gtsam { } /** \deprecated orderingType specified first for consistency */ - boost::shared_ptr eliminateSequential( + boost::shared_ptr GTSAM_DEPRECATED eliminateSequential( const Eliminate& function, OptionalVariableIndex variableIndex = boost::none, OptionalOrderingType orderingType = boost::none) const { @@ -306,7 +307,7 @@ namespace gtsam { } /** \deprecated ordering and orderingType shouldn't both be specified */ - boost::shared_ptr eliminateMultifrontal( + boost::shared_ptr GTSAM_DEPRECATED eliminateMultifrontal( const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex, @@ -315,7 +316,7 @@ namespace gtsam { } /** \deprecated orderingType specified first for consistency */ - boost::shared_ptr eliminateMultifrontal( + boost::shared_ptr GTSAM_DEPRECATED eliminateMultifrontal( const Eliminate& function, OptionalVariableIndex variableIndex = boost::none, OptionalOrderingType orderingType = boost::none) const { @@ -323,7 +324,7 @@ namespace gtsam { } /** \deprecated */ - boost::shared_ptr marginalMultifrontalBayesNet( + boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesNet( boost::variant variables, boost::none_t, const Eliminate& function = EliminationTraitsType::DefaultEliminate, @@ -332,13 +333,14 @@ namespace gtsam { } /** \deprecated */ - boost::shared_ptr marginalMultifrontalBayesTree( + boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesTree( boost::variant variables, boost::none_t, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const { return marginalMultifrontalBayesTree(variables, function, variableIndex); } + #endif }; } diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index e4a64c589..70e10b3bd 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -81,7 +81,7 @@ namespace gtsam { protected: /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType) FastVector roots_; FastVector remainingFactors_; diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 6ea81030a..c0ea4ea78 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -22,6 +22,7 @@ #pragma once #include +#include #include #include diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp index 3fe2f3307..4b30dcc08 100644 --- a/gtsam/linear/Errors.cpp +++ b/gtsam/linear/Errors.cpp @@ -111,10 +111,10 @@ double dot(const Errors& a, const Errors& b) { /* ************************************************************************* */ template<> -void axpy(double alpha, const Errors& x, Errors& y) { +void axpy(double alpha, const Errors& x, Errors& y) { Errors::const_iterator it = x.begin(); for(Vector& yi: y) - axpy(alpha,*(it++),yi); + yi += alpha * (*(it++)); } /* ************************************************************************* */ diff --git a/gtsam/linear/Errors.h b/gtsam/linear/Errors.h index eb844e04d..e8ba7344e 100644 --- a/gtsam/linear/Errors.h +++ b/gtsam/linear/Errors.h @@ -66,7 +66,7 @@ namespace gtsam { * BLAS level 2 style */ template <> - GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y); + GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y); /** print with optional string */ GTSAM_EXPORT void print(const Errors& a, const std::string& s = "Error"); diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 24c4b9a0d..664aeff6d 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -19,7 +19,6 @@ */ #include -#include #include #include #include @@ -290,10 +289,11 @@ namespace gtsam { return blocks; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues GaussianFactorGraph::optimize(const Eliminate& function) const { gttic(GaussianFactorGraph_optimize); - return BaseEliminateable::eliminateMultifrontal(function)->optimize(); + return BaseEliminateable::eliminateMultifrontal(Ordering::COLAMD, function) + ->optimize(); } /* ************************************************************************* */ @@ -503,13 +503,6 @@ namespace gtsam { return e; } - /* ************************************************************************* */ - /** \deprecated */ - VectorValues GaussianFactorGraph::optimize(boost::none_t, - const Eliminate& function) const { - return optimize(function); - } - /* ************************************************************************* */ void GaussianFactorGraph::printErrors( const VectorValues& values, const std::string& str, diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index d41374854..5ccb0ce91 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -21,12 +21,13 @@ #pragma once -#include #include +#include +#include // Included here instead of fw-declared so we can use Errors::iterator #include -#include #include -#include // Included here instead of fw-declared so we can use Errors::iterator +#include +#include namespace gtsam { @@ -395,9 +396,14 @@ namespace gtsam { public: - /** \deprecated */ - VectorValues optimize(boost::none_t, - const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 + /** \deprecated */ + VectorValues optimize(boost::none_t, + const Eliminate& function = + EliminationTraitsType::DefaultEliminate) const { + return optimize(function); + } +#endif }; diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 2fb54d329..54b9d955f 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -177,17 +177,16 @@ namespace gtsam { return *sqrt_information_; } - protected: - - /** protected constructor takes square root information matrix */ - Gaussian(size_t dim = 1, const boost::optional& sqrt_information = boost::none) : - Base(dim), sqrt_information_(sqrt_information) { - } public: typedef boost::shared_ptr shared_ptr; + /** constructor takes square root information matrix */ + Gaussian(size_t dim = 1, + const boost::optional& sqrt_information = boost::none) + : Base(dim), sqrt_information_(sqrt_information) {} + ~Gaussian() override {} /** @@ -290,13 +289,13 @@ namespace gtsam { Vector sigmas_, invsigmas_, precisions_; protected: - /** protected constructor - no initializations */ - Diagonal(); /** constructor to allow for disabling initialization of invsigmas */ Diagonal(const Vector& sigmas); public: + /** constructor - no initializations, for serialization */ + Diagonal(); typedef boost::shared_ptr shared_ptr; @@ -387,14 +386,6 @@ namespace gtsam { // Sigmas are contained in the base class Vector mu_; ///< Penalty function weight - needs to be large enough to dominate soft constraints - /** - * protected constructor takes sigmas. - * prevents any inf values - * from appearing in invsigmas or precisions. - * mu set to large default value (1000.0) - */ - Constrained(const Vector& sigmas = Z_1x1); - /** * Constructor that prevents any inf values * from appearing in invsigmas or precisions. @@ -406,6 +397,14 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; + /** + * protected constructor takes sigmas. + * prevents any inf values + * from appearing in invsigmas or precisions. + * mu set to large default value (1000.0) + */ + Constrained(const Vector& sigmas = Z_1x1); + ~Constrained() override {} /// true if a constrained noise mode, saves slow/clumsy dynamic casting @@ -461,6 +460,11 @@ namespace gtsam { return MixedVariances(precisions.array().inverse()); } + /** + * The squaredMahalanobisDistance function for a constrained noisemodel, + * for non-constrained versions, uses sigmas, otherwise + * uses the penalty function with mu + */ double squaredMahalanobisDistance(const Vector& v) const override; /** Fully constrained variations */ @@ -531,11 +535,11 @@ namespace gtsam { Isotropic(size_t dim, double sigma) : Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {} + public: + /* dummy constructor to allow for serialization */ Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {} - public: - ~Isotropic() override {} typedef boost::shared_ptr shared_ptr; @@ -592,14 +596,13 @@ namespace gtsam { * Unit: i.i.d. unit-variance noise on all m dimensions. */ class GTSAM_EXPORT Unit : public Isotropic { - protected: - - Unit(size_t dim=1): Isotropic(dim,1.0) {} - public: typedef boost::shared_ptr shared_ptr; + /** constructor for serialization */ + Unit(size_t dim=1): Isotropic(dim,1.0) {} + ~Unit() override {} /** @@ -682,19 +685,19 @@ namespace gtsam { /// Return the contained noise model const NoiseModel::shared_ptr& noise() const { return noise_; } - // TODO: functions below are dummy but necessary for the noiseModel::Base + // Functions below are dummy but necessary for the noiseModel::Base inline Vector whiten(const Vector& v) const override { Vector r = v; this->WhitenSystem(r); return r; } inline Matrix Whiten(const Matrix& A) const override { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } inline Vector unwhiten(const Vector& /*v*/) const override { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } - + /// Compute loss from the m-estimator using the Mahalanobis distance. double loss(const double squared_distance) const override { return robust_->loss(std::sqrt(squared_distance)); } - // TODO: these are really robust iterated re-weighting support functions + // These are really robust iterated re-weighting support functions virtual void WhitenSystem(Vector& b) const; void WhitenSystem(std::vector& A, Vector& b) const override; void WhitenSystem(Matrix& A, Vector& b) const override; @@ -705,7 +708,6 @@ namespace gtsam { return noise_->unweightedWhiten(v); } double weight(const Vector& v) const override { - // Todo(mikebosse): make the robust weight function input a vector. return robust_->weight(v.norm()); } diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index fdcb4f7ac..215200818 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -173,7 +173,7 @@ void SubgraphPreconditioner::transposeMultiplyAdd Errors::const_iterator it = e.begin(); for(auto& key_value: y) { const Vector& ei = *it; - axpy(alpha, ei, key_value.second); + key_value.second += alpha * ei; ++it; } transposeMultiplyAdd2(alpha, it, e.end(), y); @@ -191,7 +191,7 @@ void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha, VectorValues x = VectorValues::Zero(y); // x = 0 Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2 - axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x + y += alpha * Rc1_->backSubstituteTranspose(x); // y += alpha*inv(R1')*x } /* ************************************************************************* */ diff --git a/gtsam/linear/iterative-inl.h b/gtsam/linear/iterative-inl.h index 58ef7d733..906ee80fd 100644 --- a/gtsam/linear/iterative-inl.h +++ b/gtsam/linear/iterative-inl.h @@ -72,7 +72,7 @@ namespace gtsam { double takeOptimalStep(V& x) { // TODO: can we use gamma instead of dot(d,g) ????? Answer not trivial double alpha = -dot(d, g) / dot(Ad, Ad); // calculate optimal step-size - axpy(alpha, d, x); // // do step in new search direction, x += alpha*d + x += alpha * d; // do step in new search direction, x += alpha*d return alpha; } @@ -106,7 +106,7 @@ namespace gtsam { double beta = new_gamma / gamma; // d = g + d*beta; d *= beta; - axpy(1.0, g, d); + d += 1.0 * g; } gamma = new_gamma; diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index c74161f26..d882cb38b 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -34,6 +34,9 @@ virtual class Gaussian : gtsam::noiseModel::Base { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; virtual class Diagonal : gtsam::noiseModel::Gaussian { @@ -49,6 +52,9 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; virtual class Constrained : gtsam::noiseModel::Diagonal { @@ -66,6 +72,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; virtual class Isotropic : gtsam::noiseModel::Diagonal { @@ -78,6 +87,9 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; virtual class Unit : gtsam::noiseModel::Isotropic { @@ -85,6 +97,9 @@ virtual class Unit : gtsam::noiseModel::Isotropic { // enabling serialization functionality void serializable() const; + + // enable pickling in python + void pickle() const; }; namespace mEstimator { @@ -302,6 +317,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; void printKeys(string s) const; + gtsam::KeyVector& keys() const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; size_t size() const; Vector unweighted_error(const gtsam::VectorValues& c) const; @@ -514,9 +530,9 @@ virtual class GaussianBayesNet { size_t size() const; // FactorGraph derived interface - // size_t size() const; gtsam::GaussianConditional* at(size_t idx) const; gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; bool exists(size_t idx) const; void saveGraph(const string& s) const; diff --git a/gtsam/linear/tests/testErrors.cpp b/gtsam/linear/tests/testErrors.cpp index 74eef9a2c..f11fb90b9 100644 --- a/gtsam/linear/tests/testErrors.cpp +++ b/gtsam/linear/tests/testErrors.cpp @@ -32,7 +32,7 @@ TEST( Errors, arithmetic ) e += Vector2(1.0,2.0), Vector3(3.0,4.0,5.0); DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9); - axpy(2.0,e,e); + axpy(2.0, e, e); Errors expected; expected += Vector2(3.0,6.0), Vector3(9.0,12.0,15.0); CHECK(assert_equal(expected,e)); diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 42d68a603..b974b6cd5 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -662,25 +662,14 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone) { double dead_zone_size = 1.0; SharedNoiseModel robust = noiseModel::Robust::Create( - noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size), - Unit::Create(3)); - -/* - * TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes - * implement a loss function, and GTSAM calls the weight function to evaluate the - * total penalty, rather than calling the loss function. The weight function should be - * used during iteratively reweighted least squares optimization, but should not be used to - * evaluate the total penalty. The long-term solution is for all mEstimators to implement - * both a weight and a loss function, and for GTSAM to call the loss function when - * evaluating the total penalty. This bug causes the test below to fail, so I'm leaving it - * commented out until the underlying bug in GTSAM is fixed. - * - * for (int i = 0; i < 5; i++) { - * Vector3 error = Vector3(i, 0, 0); - * DOUBLES_EQUAL(0.5*max(0,i-1)*max(0,i-1), robust->distance(error), 1e-8); - * } - */ + noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size), + Unit::Create(3)); + for (int i = 0; i < 5; i++) { + Vector3 error = Vector3(i, 0, 0); + DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i, + robust->squaredMahalanobisDistance(error), 1e-8); + } } TEST(NoiseModel, lossFunctionAtZero) @@ -707,9 +696,9 @@ TEST(NoiseModel, lossFunctionAtZero) auto dcs = mEstimator::DCS::Create(k); DOUBLES_EQUAL(dcs->loss(0), 0, 1e-8); DOUBLES_EQUAL(dcs->weight(0), 1, 1e-8); - // auto lsdz = mEstimator::L2WithDeadZone::Create(k); - // DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8); - // DOUBLES_EQUAL(lsdz->weight(0), 1, 1e-8); + auto lsdz = mEstimator::L2WithDeadZone::Create(k); + DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8); + DOUBLES_EQUAL(lsdz->weight(0), 0, 1e-8); } diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index c5b3dab37..881b2830e 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -39,14 +39,14 @@ using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ // Export Noisemodels // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") /* ************************************************************************* */ // example noise models @@ -129,9 +129,9 @@ TEST (Serialization, SharedDiagonal_noiseModels) { /* Create GUIDs for factors */ /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional"); +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional") /* ************************************************************************* */ TEST (Serialization, linear_factors) { diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index ca1c5b93a..7d086eb57 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -257,5 +257,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) { /// namespace gtsam /// Boost serialization export definition for derived class -BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams); +BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index ed94750b7..89e8b574f 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -353,4 +353,4 @@ struct traits : public Testable {}; } // namespace gtsam /// Add Boost serialization export key (declaration) for derived class -BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams); +BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams) diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 7a879c3ef..1f9ffcf2e 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -41,6 +41,12 @@ class ConstantBias { Vector gyroscope() const; Vector correctAccelerometer(Vector measurement) const; Vector correctGyroscope(Vector measurement) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; }///\namespace imuBias @@ -64,6 +70,12 @@ class NavState { gtsam::NavState retract(const Vector& x) const; Vector localCoordinates(const gtsam::NavState& g) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -106,6 +118,12 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { Matrix getAccelerometerCovariance() const; Matrix getIntegrationCovariance() const; bool getUse2ndOrderCoriolis() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; #include @@ -135,6 +153,12 @@ class PreintegratedImuMeasurements { Vector biasHatVector() const; gtsam::NavState predict(const gtsam::NavState& state_i, const gtsam::imuBias::ConstantBias& bias) const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; virtual class ImuFactor: gtsam::NonlinearFactor { diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index d49907cbf..9304e8412 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -66,7 +66,7 @@ TEST( Rot3AttitudeFactor, Constructor ) { /* ************************************************************************* */ // Export Noisemodels // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic); +BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic) /* ************************************************************************* */ TEST(Rot3AttitudeFactor, Serialization) { diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index b784c0c94..c94e1d3d5 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -72,7 +72,7 @@ TEST( GPSFactor, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); + std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T); // Use the factor to calculate the derivative Matrix actualH; @@ -101,7 +101,7 @@ TEST( GPSFactor2, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); + std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T); // Use the factor to calculate the derivative Matrix actualH; diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp index ed72e18e9..e72b1fb5b 100644 --- a/gtsam/navigation/tests/testImuFactorSerialization.cpp +++ b/gtsam/navigation/tests/testImuFactorSerialization.cpp @@ -31,16 +31,16 @@ using namespace gtsam; using namespace gtsam::serializationTestHelpers; BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, - "gtsam_noiseModel_Constrained"); + "gtsam_noiseModel_Constrained") BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, - "gtsam_noiseModel_Diagonal"); + "gtsam_noiseModel_Diagonal") BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, - "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); + "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, - "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") template P getPreintegratedMeasurements() { diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 5107b3b6b..85447facd 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -64,7 +64,7 @@ TEST( MagFactor, unrotate ) { Point3 expected(22735.5, 314.502, 44202.5); EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6)); + (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6)); } // ************************************************************************* @@ -76,35 +76,35 @@ TEST( MagFactor, Factors ) { MagFactor f(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal((Matrix)numericalDerivative11 // - (std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); + (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); + (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// + (std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),// H1, 1e-7)); EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// + (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),// H2, 1e-7)); // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// + (std::bind(&MagFactor3::evaluateError, &f3, std::placeholders::_1, dir, bias, none, none, none), s),// H1, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// + (std::bind(&MagFactor3::evaluateError, &f3, s, std::placeholders::_1, bias, none, none, none), dir),// H2, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// + (std::bind(&MagFactor3::evaluateError, &f3, s, dir, std::placeholders::_1, none, none, none), bias),// H3, 1e-7)); } diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index dbaf31898..615b5418e 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -101,4 +101,4 @@ private: } }; -}; +} diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index c319f26e6..7e9db6b64 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -78,7 +78,8 @@ VectorValues DoglegOptimizerImpl::ComputeBlend(double delta, const VectorValues& // Compute blended point if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl; - VectorValues blend = (1. - tau) * x_u; axpy(tau, x_n, blend); + VectorValues blend = (1. - tau) * x_u; + blend += tau * x_n; return blend; } diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index b55d643aa..d22690cf3 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -305,7 +305,7 @@ struct traits> * \deprecated Prefer the more general ExpressionFactorN<>. */ template -class ExpressionFactor2 : public ExpressionFactorN { +class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN { public: /// Destructor ~ExpressionFactor2() override {} diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 3ddaf4820..3025d2468 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -142,8 +142,9 @@ class GTSAM_EXPORT GncOptimizer { * provides an extra interface for the user to initialize the weightst * */ void setWeights(const Vector w) { - if(w.size() != nfg_.size()){ - throw std::runtime_error("GncOptimizer::setWeights: the number of specified weights" + if (size_t(w.size()) != nfg_.size()) { + throw std::runtime_error( + "GncOptimizer::setWeights: the number of specified weights" " does not match the size of the factor graph."); } weights_ = w; diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 8c5b34f01..efc095775 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -29,9 +29,6 @@ protected: GaussianFactor::shared_ptr factor_; boost::optional linearizationPoint_; - /** Default constructor - necessary for serialization */ - LinearContainerFactor() {} - /** direct copy constructor */ GTSAM_EXPORT LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint); @@ -43,6 +40,9 @@ public: typedef boost::shared_ptr shared_ptr; + /** Default constructor - necessary for serialization */ + LinearContainerFactor() {} + /** Primary constructor: store a linear factor with optional linearization point */ GTSAM_EXPORT LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values()); diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index c29a79623..41212ed76 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -80,11 +80,15 @@ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solut /* ************************************************************************* */ void Marginals::computeBayesTree() { + // The default ordering to use. + const Ordering::OrderingType defaultOrderingType = Ordering::COLAMD; // Compute BayesTree - if(factorization_ == CHOLESKY) - bayesTree_ = *graph_.eliminateMultifrontal(EliminatePreferCholesky); - else if(factorization_ == QR) - bayesTree_ = *graph_.eliminateMultifrontal(EliminateQR); + if (factorization_ == CHOLESKY) + bayesTree_ = *graph_.eliminateMultifrontal(defaultOrderingType, + EliminatePreferCholesky); + else if (factorization_ == QR) + bayesTree_ = + *graph_.eliminateMultifrontal(defaultOrderingType, EliminateQR); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 9935bafdd..947274a97 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -131,17 +131,19 @@ protected: void computeBayesTree(const Ordering& ordering); public: +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** \deprecated argument order changed due to removing boost::optional */ - Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, + GTSAM_DEPRECATED Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} /** \deprecated argument order changed due to removing boost::optional */ - Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, + GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} /** \deprecated argument order changed due to removing boost::optional */ - Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, + GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} +#endif }; diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index fd9e49a62..a7a0d724b 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -149,7 +149,7 @@ boost::tuple nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) { - // GTSAM_CONCEPT_MANIFOLD_TYPE(V); + // GTSAM_CONCEPT_MANIFOLD_TYPE(V) size_t iteration = 0; diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 47083d5d7..43d30254e 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -219,7 +219,6 @@ protected: X value_; /// fixed value for variable GTSAM_CONCEPT_MANIFOLD_TYPE(X) - GTSAM_CONCEPT_TESTABLE_TYPE(X) public: diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 8b8d2da6c..3d572e970 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -114,7 +114,7 @@ double NoiseModelFactor::weight(const Values& c) const { if (noiseModel_) { const Vector b = unwhitenedError(c); check(noiseModel_, b.size()); - return 0.5 * noiseModel_->weight(b); + return noiseModel_->weight(b); } else return 1.0; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 4d321f8ab..61cbbafb9 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -265,15 +265,17 @@ namespace gtsam { public: +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** \deprecated */ - boost::shared_ptr linearizeToHessianFactor( + boost::shared_ptr GTSAM_DEPRECATED linearizeToHessianFactor( const Values& values, boost::none_t, const Dampen& dampen = nullptr) const {return linearizeToHessianFactor(values, dampen);} /** \deprecated */ - Values updateCholesky(const Values& values, boost::none_t, + Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t, const Dampen& dampen = nullptr) const {return updateCholesky(values, dampen);} +#endif }; diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 0d7e9e17f..3ce6db4af 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -147,11 +147,13 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) if (params.ordering) - delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), - boost::none, params.orderingType)->optimize(); + delta = gfg.eliminateSequential(*params.ordering, + params.getEliminationFunction()) + ->optimize(); else - delta = gfg.eliminateSequential(params.getEliminationFunction(), boost::none, - params.orderingType)->optimize(); + delta = gfg.eliminateSequential(params.orderingType, + params.getEliminationFunction()) + ->optimize(); } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams if (!params.iterativeParams) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 33e9e7d82..207f35540 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -24,6 +24,7 @@ #pragma once +#include #include #include #include @@ -62,17 +63,18 @@ namespace gtsam { class GTSAM_EXPORT Values { private: - // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined - // below) to clone and deallocate the Value objects, and a boost - // fast_pool_allocator to allocate map nodes. In this way, all memory is - // allocated in a boost memory pool. + // below) to clone and deallocate the Value objects, and our compile-flag- + // dependent FastDefaultAllocator to allocate map nodes. In this way, the + // user defines the allocation details (i.e. optimize for memory pool/arenas + // concurrency). + typedef internal::FastDefaultAllocator>::type KeyValuePtrPairAllocator; typedef boost::ptr_map< Key, Value, std::less, ValueCloneAllocator, - boost::fast_pool_allocator > > KeyValueMap; + KeyValuePtrPairAllocator > KeyValueMap; // The member to store the values, see just above KeyValueMap values_; diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h index 74ef87737..3a9b6fb11 100644 --- a/gtsam/nonlinear/factorTesting.h +++ b/gtsam/nonlinear/factorTesting.h @@ -35,8 +35,7 @@ namespace gtsam { * zero errors anyway. However, it means that below will only be exact for the correct measurement. */ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, - const Values& values, double delta = 1e-5) { - + const Values& values, double delta = 1e-5) { // We will fill a vector of key/Jacobians pairs (a map would sort) std::vector > jacobians; @@ -46,24 +45,24 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, // Loop over all variables const double one_over_2delta = 1.0 / (2.0 * delta); - for(Key key: factor) { + for (Key key : factor) { // Compute central differences using the values struct. VectorValues dX = values.zeroVectors(); const size_t cols = dX.dim(key); Matrix J = Matrix::Zero(rows, cols); for (size_t col = 0; col < cols; ++col) { Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); - dx[col] = delta; + dx(col) = delta; dX[key] = dx; Values eval_values = values.retract(dX); const Eigen::VectorXd left = factor.whitenedError(eval_values); - dx[col] = -delta; + dx(col) = -delta; dX[key] = dx; eval_values = values.retract(dX); const Eigen::VectorXd right = factor.whitenedError(eval_values); J.col(col) = (left - right) * one_over_2delta; } - jacobians.push_back(std::make_pair(key,J)); + jacobians.push_back(std::make_pair(key, J)); } // Next step...return JacobianFactor diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index d068bd7ee..152c4b8e7 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -115,6 +115,10 @@ class Ordering { Ordering(); Ordering(const gtsam::Ordering& other); + template + static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); + // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; @@ -734,7 +738,12 @@ class ISAM2 { const gtsam::KeyList& extraReelimKeys, bool force_relinearize); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::ISAM2UpdateParams& updateParams); + gtsam::Values getLinearizationPoint() const; + bool valueExists(gtsam::Key key) const; gtsam::Values calculateEstimate() const; template , gtsam::PinholeCamera, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; - gtsam::Values calculateBestEstimate() const; Matrix marginalCovariance(size_t key) const; + gtsam::Values calculateBestEstimate() const; gtsam::VectorValues getDelta() const; + double error(const gtsam::VectorValues& x) const; gtsam::NonlinearFactorGraph getFactorsUnsafe() const; gtsam::VariableIndex getVariableIndex() const; + const gtsam::KeySet& getFixedVariables() const; gtsam::ISAM2Params params() const; + + void printStats() const; + gtsam::VectorValues gradientAtZero() const; }; #include diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index f4bb5f4f6..4a73cbb0b 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -35,37 +35,37 @@ using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ // Create GUIDs for Noisemodels -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") /* ************************************************************************* */ // Create GUIDs for factors -BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor, "gtsam::PriorFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional"); +BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor, "gtsam::PriorFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional") /* ************************************************************************* */ // Export all classes derived from Value -GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); -GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler); -GTSAM_VALUE_EXPORT(gtsam::Point3); -GTSAM_VALUE_EXPORT(gtsam::Pose3); -GTSAM_VALUE_EXPORT(gtsam::Rot3); -GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); -GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); -GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2) +GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler) +GTSAM_VALUE_EXPORT(gtsam::Point3) +GTSAM_VALUE_EXPORT(gtsam::Pose3) +GTSAM_VALUE_EXPORT(gtsam::Rot3) +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera) +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera) +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera) namespace detail { template struct pack { diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 2e4543177..bfc3a0f78 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -59,8 +59,8 @@ namespace gtsam { template class GeneralSFMFactor: public NoiseModelFactor2 { - GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA); - GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK); + GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) + GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) static const int DimC = FixedDimension::value; static const int DimL = FixedDimension::value; @@ -202,7 +202,7 @@ struct traits > : Testable< template class GeneralSFMFactor2: public NoiseModelFactor3 { - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) static const int DimK = FixedDimension::value; protected: diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index c8a8b15c5..d7e925bd9 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -392,7 +392,7 @@ parseMeasurements(const std::string &filename, size_t maxIndex) { ParseMeasurement parse{model ? createSampler(model) : nullptr, maxIndex, true, NoiseFormatAUTO, - KernelFunctionTypeNONE}; + KernelFunctionTypeNONE, nullptr}; return parseToVector>(filename, parse); } diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 60000dbab..527e838b2 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -168,6 +168,13 @@ template virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); + POSE measured() const; + + // enabling serialization functionality + void serialize() const; + + // enable pickling in python + void pickle() const; }; typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; @@ -178,6 +185,7 @@ template virtual class PoseRotationPrior : gtsam::NoiseModelFactor { PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); + POSE measured() const; }; typedef gtsam::PoseRotationPrior PoseRotationPrior2D; diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index 951cbf8f4..ba46dce8d 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -127,13 +127,13 @@ TEST(SmartFactorBase, Stereo) { } /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") TEST(SmartFactorBase, serialize) { using namespace gtsam::serializationTestHelpers; diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 1fd06cc9f..133f81511 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -811,13 +811,13 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) { } /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") TEST(SmartProjectionFactor, serialize) { using namespace vanilla; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 997c33846..f3706fa02 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -1333,13 +1333,13 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { } /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") TEST(SmartProjectionPoseFactor, serialize) { using namespace vanillaPose; diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index 9e124954f..85cf0b472 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -5,107 +5,109 @@ * @author Frank Dellaert */ -#include -#include #include +#include +#include + #include namespace gtsam { - /* ************************************************************************* */ - AllDiff::AllDiff(const DiscreteKeys& dkeys) : - Constraint(dkeys.indices()) { - for(const DiscreteKey& dkey: dkeys) - cardinalities_.insert(dkey); - } +/* ************************************************************************* */ +AllDiff::AllDiff(const DiscreteKeys& dkeys) : Constraint(dkeys.indices()) { + for (const DiscreteKey& dkey : dkeys) cardinalities_.insert(dkey); +} - /* ************************************************************************* */ - void AllDiff::print(const std::string& s, - const KeyFormatter& formatter) const { - std::cout << s << "AllDiff on "; - for (Key dkey: keys_) - std::cout << formatter(dkey) << " "; - std::cout << std::endl; - } +/* ************************************************************************* */ +void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const { + std::cout << s << "AllDiff on "; + for (Key dkey : keys_) std::cout << formatter(dkey) << " "; + std::cout << std::endl; +} - /* ************************************************************************* */ - double AllDiff::operator()(const Values& values) const { - std::set < size_t > taken; // record values taken by keys - for(Key dkey: keys_) { - size_t value = values.at(dkey); // get the value for that key - if (taken.count(value)) return 0.0;// check if value alreday taken - taken.insert(value);// if not, record it as taken and keep checking +/* ************************************************************************* */ +double AllDiff::operator()(const Values& values) const { + std::set taken; // record values taken by keys + for (Key dkey : keys_) { + size_t value = values.at(dkey); // get the value for that key + if (taken.count(value)) return 0.0; // check if value alreday taken + taken.insert(value); // if not, record it as taken and keep checking + } + return 1.0; +} + +/* ************************************************************************* */ +DecisionTreeFactor AllDiff::toDecisionTreeFactor() const { + // We will do this by converting the allDif into many BinaryAllDiff + // constraints + DecisionTreeFactor converted; + size_t nrKeys = keys_.size(); + for (size_t i1 = 0; i1 < nrKeys; i1++) + for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) { + BinaryAllDiff binary12(discreteKey(i1), discreteKey(i2)); + converted = converted * binary12.toDecisionTreeFactor(); } - return 1.0; + return converted; +} + +/* ************************************************************************* */ +DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; +} + +/* ************************************************************************* */ +bool AllDiff::ensureArcConsistency(Key j, Domains* domains) const { + Domain& Dj = domains->at(j); + + // Though strictly not part of allDiff, we check for + // a value in domains->at(j) that does not occur in any other connected domain. + // If found, we make this a singleton... + // TODO: make a new constraint where this really is true + boost::optional maybeChanged = Dj.checkAllDiff(keys_, *domains); + if (maybeChanged) { + Dj = *maybeChanged; + return true; } - /* ************************************************************************* */ - DecisionTreeFactor AllDiff::toDecisionTreeFactor() const { - // We will do this by converting the allDif into many BinaryAllDiff constraints - DecisionTreeFactor converted; - size_t nrKeys = keys_.size(); - for (size_t i1 = 0; i1 < nrKeys; i1++) - for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) { - BinaryAllDiff binary12(discreteKey(i1),discreteKey(i2)); - converted = converted * binary12.toDecisionTreeFactor(); - } - return converted; - } - - /* ************************************************************************* */ - DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* ************************************************************************* */ - bool AllDiff::ensureArcConsistency(size_t j, std::vector& domains) const { - // Though strictly not part of allDiff, we check for - // a value in domains[j] that does not occur in any other connected domain. - // If found, we make this a singleton... - // TODO: make a new constraint where this really is true - Domain& Dj = domains[j]; - if (Dj.checkAllDiff(keys_, domains)) return true; - - // Check all other domains for singletons and erase corresponding values - // This is the same as arc-consistency on the equivalent binary constraints - bool changed = false; - for(Key k: keys_) - if (k != j) { - const Domain& Dk = domains[k]; - if (Dk.isSingleton()) { // check if singleton - size_t value = Dk.firstValue(); - if (Dj.contains(value)) { - Dj.erase(value); // erase value if true - changed = true; - } + // Check all other domains for singletons and erase corresponding values. + // This is the same as arc-consistency on the equivalent binary constraints + bool changed = false; + for (Key k : keys_) + if (k != j) { + const Domain& Dk = domains->at(k); + if (Dk.isSingleton()) { // check if singleton + size_t value = Dk.firstValue(); + if (Dj.contains(value)) { + Dj.erase(value); // erase value if true + changed = true; } } - return changed; - } + } + return changed; +} - /* ************************************************************************* */ - Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const { - DiscreteKeys newKeys; - // loop over keys and add them only if they do not appear in values - for(Key k: keys_) - if (values.find(k) == values.end()) { - newKeys.push_back(DiscreteKey(k,cardinalities_.at(k))); - } - return boost::make_shared(newKeys); - } +/* ************************************************************************* */ +Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const { + DiscreteKeys newKeys; + // loop over keys and add them only if they do not appear in values + for (Key k : keys_) + if (values.find(k) == values.end()) { + newKeys.push_back(DiscreteKey(k, cardinalities_.at(k))); + } + return boost::make_shared(newKeys); +} - /* ************************************************************************* */ - Constraint::shared_ptr AllDiff::partiallyApply( - const std::vector& domains) const { - DiscreteFactor::Values known; - for(Key k: keys_) { - const Domain& Dk = domains[k]; - if (Dk.isSingleton()) - known[k] = Dk.firstValue(); - } - return partiallyApply(known); +/* ************************************************************************* */ +Constraint::shared_ptr AllDiff::partiallyApply( + const Domains& domains) const { + DiscreteFactor::Values known; + for (Key k : keys_) { + const Domain& Dk = domains.at(k); + if (Dk.isSingleton()) known[k] = Dk.firstValue(); } + return partiallyApply(known); +} - /* ************************************************************************* */ -} // namespace gtsam +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 80e700b29..57b0aeb5c 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -7,71 +7,66 @@ #pragma once -#include #include +#include namespace gtsam { - /** - * General AllDiff constraint - * Returns 1 if values for all keys are different, 0 otherwise - * DiscreteFactors are all awkward in that they have to store two types of keys: - * for each variable we have a Key and an Key. In this factor, we - * keep the Indices locally, and the Indices are stored in IndexFactor. +/** + * General AllDiff constraint. + * Returns 1 if values for all keys are different, 0 otherwise. + */ +class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { + std::map cardinalities_; + + DiscreteKey discreteKey(size_t i) const { + Key j = keys_[i]; + return DiscreteKey(j, cardinalities_.at(j)); + } + + public: + /// Construct from keys. + AllDiff(const DiscreteKeys& dkeys); + + // print + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + 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 ! + double operator()(const Values& values) const override; + + /// Convert into a decisiontree, can be *very* expensive ! + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + /* + * Ensure Arc-consistency by checking every possible value of domain j. + * @param j domain to be checked + * @param (in/out) domains all domains, but only domains->at(j) will be checked. + * @return true if domains->at(j) was changed, false otherwise. */ - class GTSAM_UNSTABLE_EXPORT AllDiff: public Constraint { + bool ensureArcConsistency(Key j, Domains* domains) const override; - std::map cardinalities_; + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values&) const override; - DiscreteKey discreteKey(size_t i) const { - Key j = keys_[i]; - return DiscreteKey(j,cardinalities_.at(j)); - } + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const Domains&) const override; +}; - public: - - /// Constructor - AllDiff(const DiscreteKeys& dkeys); - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - 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 ! - double operator()(const Values& values) const override; - - /// Convert into a decisiontree, can be *very* expensive ! - DecisionTreeFactor toDecisionTreeFactor() const override; - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; - - /* - * Ensure Arc-consistency - * Arc-consistency involves creating binaryAllDiff constraints - * In which case the combinatorial hyper-arc explosion disappears. - * @param j domain to be checked - * @param domains all other domains - */ - bool ensureArcConsistency(size_t j, std::vector& domains) const override; - - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values&) const override; - - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply(const std::vector&) const override; - }; - -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index bbb60e2f1..a2c7ba660 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -7,94 +7,90 @@ #pragma once -#include -#include #include +#include +#include namespace gtsam { - /** - * Binary AllDiff constraint - * Returns 1 if values for two keys are different, 0 otherwise - * DiscreteFactors are all awkward in that they have to store two types of keys: - * for each variable we have a Index and an Index. In this factor, we - * keep the Indices locally, and the Indices are stored in IndexFactor. - */ - class BinaryAllDiff: public Constraint { +/** + * Binary AllDiff constraint + * Returns 1 if values for two keys are different, 0 otherwise. + */ +class BinaryAllDiff : public Constraint { + size_t cardinality0_, cardinality1_; /// cardinality - size_t cardinality0_, cardinality1_; /// cardinality + public: + /// Constructor + BinaryAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) + : Constraint(key1.first, key2.first), + cardinality0_(key1.second), + cardinality1_(key2.second) {} - public: + // print + void print( + const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " + << formatter(keys_[1]) << std::endl; + } - /// Constructor - BinaryAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) : - Constraint(key1.first, key2.first), - cardinality0_(key1.second), cardinality1_(key2.second) { - } - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override { - std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " - << formatter(keys_[1]) << std::endl; - } - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if(!dynamic_cast(&other)) - return false; - else { - const BinaryAllDiff& f(static_cast(other)); - return (cardinality0_==f.cardinality0_) && (cardinality1_==f.cardinality1_); - } - } - - /// Calculate value - double operator()(const Values& values) const override { - return (double) (values.at(keys_[0]) != values.at(keys_[1])); - } - - /// Convert into a decisiontree - DecisionTreeFactor toDecisionTreeFactor() const override { - DiscreteKeys keys; - keys.push_back(DiscreteKey(keys_[0],cardinality0_)); - keys.push_back(DiscreteKey(keys_[1],cardinality1_)); - std::vector table; - for (size_t i1 = 0; i1 < cardinality0_; i1++) - for (size_t i2 = 0; i2 < cardinality1_; i2++) - table.push_back(i1 != i2); - DecisionTreeFactor converted(keys, table); - return converted; - } - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains - */ - /// - bool ensureArcConsistency(size_t j, std::vector& domains) const override { -// throw std::runtime_error( -// "BinaryAllDiff::ensureArcConsistency not implemented"); + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) return false; + else { + const BinaryAllDiff& f(static_cast(other)); + return (cardinality0_ == f.cardinality0_) && + (cardinality1_ == f.cardinality1_); } + } - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values&) const override { - throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); - } + /// Calculate value + double operator()(const Values& values) const override { + return (double)(values.at(keys_[0]) != values.at(keys_[1])); + } - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector&) const override { - throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); - } - }; + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override { + DiscreteKeys keys; + keys.push_back(DiscreteKey(keys_[0], cardinality0_)); + keys.push_back(DiscreteKey(keys_[1], cardinality1_)); + std::vector table; + for (size_t i1 = 0; i1 < cardinality0_; i1++) + for (size_t i2 = 0; i2 < cardinality1_; i2++) table.push_back(i1 != i2); + DecisionTreeFactor converted(keys, table); + return converted; + } -} // namespace gtsam + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; + } + + /* + * Ensure Arc-consistency by checking every possible value of domain j. + * @param j domain to be checked + * @param (in/out) domains all domains, but only domains->at(j) will be checked. + * @return true if domains->at(j) was changed, false otherwise. + */ + bool ensureArcConsistency(Key j, Domains* domains) const override { + throw std::runtime_error( + "BinaryAllDiff::ensureArcConsistency not implemented"); + return false; + } + + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values&) const override { + throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); + } + + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const Domains&) const override { + throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); + } +}; + +} // namespace gtsam diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index 525abd098..1b38d20d6 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -5,99 +5,95 @@ * @author Frank Dellaert */ -#include -#include #include +#include +#include using namespace std; namespace gtsam { - /// Find the best total assignment - can be expensive - CSP::sharedValues CSP::optimalAssignment() const { - DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(); - sharedValues mpe = chordal->optimize(); - return mpe; - } +/// Find the best total assignment - can be expensive +CSP::Values CSP::optimalAssignment() const { + DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(); + return chordal->optimize(); +} - /// Find the best total assignment - can be expensive - CSP::sharedValues CSP::optimalAssignment(const Ordering& ordering) const { - DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(ordering); - sharedValues mpe = chordal->optimize(); - return mpe; - } +/// Find the best total assignment - can be expensive +CSP::Values CSP::optimalAssignment(const Ordering& ordering) const { + DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(ordering); + return chordal->optimize(); +} - void CSP::runArcConsistency(size_t cardinality, size_t nrIterations, bool print) const { - // Create VariableIndex - VariableIndex index(*this); - // index.print(); +bool CSP::runArcConsistency(const VariableIndex& index, + Domains* domains) const { + bool changed = false; - size_t n = index.size(); + // iterate over all variables in the index + for (auto entry : index) { + // Get the variable's key and associated factors: + const Key key = entry.first; + const FactorIndices& factors = entry.second; - // Initialize domains - std::vector < Domain > domains; - for (size_t j = 0; j < n; j++) - domains.push_back(Domain(DiscreteKey(j,cardinality))); + // If this domain is already a singleton, we do nothing. + if (domains->at(key).isSingleton()) continue; - // Create array of flags indicating a domain changed or not - std::vector changed(n); - - // iterate nrIterations over entire grid - for (size_t it = 0; it < nrIterations; it++) { - bool anyChange = false; - // iterate over all cells - for (size_t v = 0; v < n; v++) { - // keep track of which domains changed - changed[v] = false; - // loop over all factors/constraints for variable v - const FactorIndices& factors = index[v]; - for(size_t f: factors) { - // if not already a singleton - if (!domains[v].isSingleton()) { - // get the constraint and call its ensureArcConsistency method - Constraint::shared_ptr constraint = boost::dynamic_pointer_cast((*this)[f]); - if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor"); - changed[v] = constraint->ensureArcConsistency(v,domains) || changed[v]; - } - } // f - if (changed[v]) anyChange = true; - } // v - if (!anyChange) break; - // TODO: Sudoku specific hack - if (print) { - if (cardinality == 9 && n == 81) { - for (size_t i = 0, v = 0; i < (size_t)std::sqrt((double)n); i++) { - for (size_t j = 0; j < (size_t)std::sqrt((double)n); j++, v++) { - if (changed[v]) cout << "*"; - domains[v].print(); - cout << "\t"; - } // i - cout << endl; - } // j - } else { - for (size_t v = 0; v < n; v++) { - if (changed[v]) cout << "*"; - domains[v].print(); - cout << "\t"; - } // v - } - cout << endl; - } // print - } // it - -#ifndef INPROGRESS - // Now create new problem with all singleton variables removed - // We do this by adding simplifying all factors using parial application - // TODO: create a new ordering as we go, to ensure a connected graph - // KeyOrdering ordering; - // vector dkeys; - for(const DiscreteFactor::shared_ptr& f: factors_) { - Constraint::shared_ptr constraint = boost::dynamic_pointer_cast(f); - if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor"); - Constraint::shared_ptr reduced = constraint->partiallyApply(domains); - if (print) reduced->print(); + // Otherwise, loop over all factors/constraints for variable with given key. + for (size_t f : factors) { + // If this factor is a constraint, call its ensureArcConsistency method: + auto constraint = boost::dynamic_pointer_cast((*this)[f]); + if (constraint) { + changed = constraint->ensureArcConsistency(key, domains) || changed; + } } -#endif } -} // gtsam + return changed; +} +// TODO(dellaert): This is AC1, which is inefficient as any change will cause +// the algorithm to revisit *all* variables again. Implement AC3. +Domains CSP::runArcConsistency(size_t cardinality, size_t maxIterations) const { + // Create VariableIndex + VariableIndex index(*this); + + // Initialize domains + Domains domains; + for (auto entry : index) { + const Key key = entry.first; + domains.emplace(key, DiscreteKey(key, cardinality)); + } + + // Iterate until convergence or not a single domain changed. + for (size_t it = 0; it < maxIterations; it++) { + bool changed = runArcConsistency(index, &domains); + if (!changed) break; + } + return domains; +} + +CSP CSP::partiallyApply(const Domains& domains) const { + // Create new problem with all singleton variables removed + // We do this by adding simplifying all factors using partial application. + // TODO: create a new ordering as we go, to ensure a connected graph + // KeyOrdering ordering; + // vector dkeys; + CSP new_csp; + + // Add tightened domains as new factors: + for (auto key_domain : domains) { + new_csp.emplace_shared(key_domain.second); + } + + // Reduce all existing factors: + for (const DiscreteFactor::shared_ptr& f : factors_) { + auto constraint = boost::dynamic_pointer_cast(f); + if (!constraint) + throw runtime_error("CSP:runArcConsistency: non-constraint factor"); + Constraint::shared_ptr reduced = constraint->partiallyApply(domains); + if (reduced->size() > 1) { + new_csp.push_back(reduced); + } + } + return new_csp; +} +} // namespace gtsam diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index 9e843f667..0d4e0b177 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -7,84 +7,78 @@ #pragma once +#include #include #include -#include namespace gtsam { - /** - * Constraint Satisfaction Problem class - * A specialization of a DiscreteFactorGraph. - * It knows about CSP-specific constraints and algorithms +/** + * Constraint Satisfaction Problem class + * A specialization of a DiscreteFactorGraph. + * It knows about CSP-specific constraints and algorithms + */ +class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph { + public: + /** A map from keys to values */ + typedef Assignment Values; + + public: + /// Add a unary constraint, allowing only a single value + void addSingleValue(const DiscreteKey& dkey, size_t value) { + emplace_shared(dkey, value); + } + + /// Add a binary AllDiff constraint + void addAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) { + emplace_shared(key1, key2); + } + + /// Add a general AllDiff constraint + void addAllDiff(const DiscreteKeys& dkeys) { emplace_shared(dkeys); } + + // /** return product of all factors as a single factor */ + // DecisionTreeFactor product() const { + // DecisionTreeFactor result; + // for(const sharedFactor& factor: *this) + // if (factor) result = (*factor) * result; + // return result; + // } + + /// Find the best total assignment - can be expensive. + Values optimalAssignment() const; + + /// Find the best total assignment, with given ordering - can be expensive. + Values optimalAssignment(const Ordering& ordering) const; + + // /* + // * Perform loopy belief propagation + // * True belief propagation would check for each value in domain + // * whether any satisfying separator assignment can be found. + // * This corresponds to hyper-arc consistency in CSP speak. + // * This can be done by creating a mini-factor graph and search. + // * For a nine-by-nine Sudoku, the search tree will be 8+6+6=20 levels + // deep. + // * It will be very expensive to exclude values that way. + // */ + // void applyBeliefPropagation(size_t maxIterations = 10) const; + + /* + * Apply arc-consistency ~ Approximate loopy belief propagation + * We need to give the domains to a constraint, and it returns + * a domain whose values don't conflict in the arc-consistency way. + * TODO: should get cardinality from DiscreteKeys */ - class GTSAM_UNSTABLE_EXPORT CSP: public DiscreteFactorGraph { - public: + Domains runArcConsistency(size_t cardinality, + size_t maxIterations = 10) const; - /** A map from keys to values */ - typedef KeyVector Indices; - typedef Assignment Values; - typedef boost::shared_ptr sharedValues; + /// Run arc consistency for all variables, return true if any domain changed. + bool runArcConsistency(const VariableIndex& index, Domains* domains) const; - public: - -// /// Constructor -// CSP() { -// } - - /// Add a unary constraint, allowing only a single value - void addSingleValue(const DiscreteKey& dkey, size_t value) { - boost::shared_ptr factor(new SingleValue(dkey, value)); - push_back(factor); - } - - /// Add a binary AllDiff constraint - void addAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) { - boost::shared_ptr factor( - new BinaryAllDiff(key1, key2)); - push_back(factor); - } - - /// Add a general AllDiff constraint - void addAllDiff(const DiscreteKeys& dkeys) { - boost::shared_ptr factor(new AllDiff(dkeys)); - push_back(factor); - } - -// /** return product of all factors as a single factor */ -// DecisionTreeFactor product() const { -// DecisionTreeFactor result; -// for(const sharedFactor& factor: *this) -// if (factor) result = (*factor) * result; -// return result; -// } - - /// Find the best total assignment - can be expensive - sharedValues optimalAssignment() const; - - /// Find the best total assignment - can be expensive - sharedValues optimalAssignment(const Ordering& ordering) const; - -// /* -// * Perform loopy belief propagation -// * True belief propagation would check for each value in domain -// * whether any satisfying separator assignment can be found. -// * This corresponds to hyper-arc consistency in CSP speak. -// * This can be done by creating a mini-factor graph and search. -// * For a nine-by-nine Sudoku, the search tree will be 8+6+6=20 levels deep. -// * It will be very expensive to exclude values that way. -// */ -// void applyBeliefPropagation(size_t nrIterations = 10) const; - - /* - * Apply arc-consistency ~ Approximate loopy belief propagation - * We need to give the domains to a constraint, and it returns - * a domain whose values don't conflict in the arc-consistency way. - * TODO: should get cardinality from Indices - */ - void runArcConsistency(size_t cardinality, size_t nrIterations = 10, - bool print = false) const; - }; // CSP - -} // gtsam + /* + * Create a new CSP, applying the given Domain constraints. + */ + CSP partiallyApply(const Domains& domains) const; +}; // CSP +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index c3a26de68..f0e51b723 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -17,77 +17,70 @@ #pragma once -#include #include +#include + #include +#include namespace gtsam { - class Domain; +class Domain; +using Domains = std::map; - /** - * Base class for discrete probabilistic factors - * The most general one is the derived DecisionTreeFactor +/** + * Base class for constraint factors + * Derived classes include SingleValue, BinaryAllDiff, and AllDiff. + */ +class GTSAM_EXPORT Constraint : public DiscreteFactor { + public: + typedef boost::shared_ptr shared_ptr; + + protected: + /// Construct unary constraint factor. + Constraint(Key j) : DiscreteFactor(boost::assign::cref_list_of<1>(j)) {} + + /// Construct binary constraint factor. + Constraint(Key j1, Key j2) + : DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) {} + + /// Construct n-way constraint factor. + Constraint(const KeyVector& js) : DiscreteFactor(js) {} + + /// construct from container + template + Constraint(KeyIterator beginKey, KeyIterator endKey) + : DiscreteFactor(beginKey, endKey) {} + + public: + /// @name Standard Constructors + /// @{ + + /// Default constructor for I/O + Constraint(); + + /// Virtual destructor + ~Constraint() override {} + + /// @} + /// @name Standard Interface + /// @{ + + /* + * Ensure Arc-consistency by checking every possible value of domain j. + * @param j domain to be checked + * @param (in/out) domains all domains, but only domains->at(j) will be checked. + * @return true if domains->at(j) was changed, false otherwise. */ - class Constraint : public DiscreteFactor { + virtual bool ensureArcConsistency(Key j, Domains* domains) const = 0; - public: + /// Partially apply known values + virtual shared_ptr partiallyApply(const Values&) const = 0; - typedef boost::shared_ptr shared_ptr; - - protected: - - /// Construct n-way factor - Constraint(const KeyVector& js) : - DiscreteFactor(js) { - } - - /// Construct unary factor - Constraint(Key j) : - DiscreteFactor(boost::assign::cref_list_of<1>(j)) { - } - - /// Construct binary factor - Constraint(Key j1, Key j2) : - DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) { - } - - /// construct from container - template - Constraint(KeyIterator beginKey, KeyIterator endKey) : - DiscreteFactor(beginKey, endKey) { - } - - public: - - /// @name Standard Constructors - /// @{ - - /// Default constructor for I/O - Constraint(); - - /// Virtual destructor - ~Constraint() override {} - - /// @} - /// @name Standard Interface - /// @{ - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains - */ - virtual bool ensureArcConsistency(size_t j, std::vector& domains) const = 0; - - /// Partially apply known values - virtual shared_ptr partiallyApply(const Values&) const = 0; - - - /// Partially apply known values, domain version - virtual shared_ptr partiallyApply(const std::vector&) const = 0; - /// @} - }; + /// Partially apply known values, domain version + virtual shared_ptr partiallyApply(const Domains&) const = 0; + /// @} +}; // DiscreteFactor -}// namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index 740ef067c..98b735c6c 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -5,92 +5,94 @@ * @author Frank Dellaert */ -#include -#include #include -#include +#include +#include +#include +#include namespace gtsam { - using namespace std; - - /* ************************************************************************* */ - void Domain::print(const string& s, - const KeyFormatter& formatter) const { -// cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" << -// formatter(keys_[0]) << ") with values"; -// for (size_t v: values_) cout << " " << v; -// cout << endl; - for (size_t v: values_) cout << v; - } - - /* ************************************************************************* */ - double Domain::operator()(const Values& values) const { - return contains(values.at(keys_[0])); - } - - /* ************************************************************************* */ - DecisionTreeFactor Domain::toDecisionTreeFactor() const { - DiscreteKeys keys; - keys += DiscreteKey(keys_[0],cardinality_); - vector table; - for (size_t i1 = 0; i1 < cardinality_; ++i1) - table.push_back(contains(i1)); - DecisionTreeFactor converted(keys, table); - return converted; - } - - /* ************************************************************************* */ - DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* ************************************************************************* */ - bool Domain::ensureArcConsistency(size_t j, vector& domains) const { - if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain"); - Domain& D = domains[j]; - for(size_t value: values_) - if (!D.contains(value)) throw runtime_error("Unsatisfiable"); - D = *this; - return true; - } - - /* ************************************************************************* */ - bool Domain::checkAllDiff(const KeyVector keys, vector& domains) { - Key j = keys_[0]; - // for all values in this domain - for(size_t value: values_) { - // for all connected domains - for(Key k: keys) - // if any domain contains the value we cannot make this domain singleton - if (k!=j && domains[k].contains(value)) - goto found; - values_.clear(); - values_.insert(value); - return true; // we changed it - found:; - } - return false; // we did not change it - } - - /* ************************************************************************* */ - Constraint::shared_ptr Domain::partiallyApply( - const Values& values) const { - Values::const_iterator it = values.find(keys_[0]); - if (it != values.end() && !contains(it->second)) throw runtime_error( - "Domain::partiallyApply: unsatisfiable"); - return boost::make_shared < Domain > (*this); - } - - /* ************************************************************************* */ - Constraint::shared_ptr Domain::partiallyApply( - const vector& domains) const { - const Domain& Dk = domains[keys_[0]]; - if (Dk.isSingleton() && !contains(*Dk.begin())) throw runtime_error( - "Domain::partiallyApply: unsatisfiable"); - return boost::make_shared < Domain > (Dk); - } +using namespace std; /* ************************************************************************* */ -} // namespace gtsam +void Domain::print(const string& s, const KeyFormatter& formatter) const { + cout << s << ": Domain on " << formatter(key()) << " (j=" << formatter(key()) + << ") with values"; + for (size_t v : values_) cout << " " << v; + cout << endl; +} + +/* ************************************************************************* */ +string Domain::base1Str() const { + stringstream ss; + for (size_t v : values_) ss << v + 1; + return ss.str(); +} + +/* ************************************************************************* */ +double Domain::operator()(const Values& values) const { + return contains(values.at(key())); +} + +/* ************************************************************************* */ +DecisionTreeFactor Domain::toDecisionTreeFactor() const { + DiscreteKeys keys; + keys += DiscreteKey(key(), cardinality_); + vector table; + for (size_t i1 = 0; i1 < cardinality_; ++i1) table.push_back(contains(i1)); + DecisionTreeFactor converted(keys, table); + return converted; +} + +/* ************************************************************************* */ +DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; +} + +/* ************************************************************************* */ +bool Domain::ensureArcConsistency(Key j, Domains* domains) const { + if (j != key()) throw invalid_argument("Domain check on wrong domain"); + Domain& D = domains->at(j); + for (size_t value : values_) + if (!D.contains(value)) throw runtime_error("Unsatisfiable"); + D = *this; + return true; +} + +/* ************************************************************************* */ +boost::optional Domain::checkAllDiff(const KeyVector keys, + const Domains& domains) const { + Key j = key(); + // for all values in this domain + for (const size_t value : values_) { + // for all connected domains + for (const Key k : keys) + // if any domain contains the value we cannot make this domain singleton + if (k != j && domains.at(k).contains(value)) goto found; + // Otherwise: return a singleton: + return Domain(this->discreteKey(), value); + found:; + } + return boost::none; // we did not change it +} + +/* ************************************************************************* */ +Constraint::shared_ptr Domain::partiallyApply(const Values& values) const { + Values::const_iterator it = values.find(key()); + if (it != values.end() && !contains(it->second)) + throw runtime_error("Domain::partiallyApply: unsatisfiable"); + return boost::make_shared(*this); +} + +/* ************************************************************************* */ +Constraint::shared_ptr Domain::partiallyApply(const Domains& domains) const { + const Domain& Dk = domains.at(key()); + if (Dk.isSingleton() && !contains(*Dk.begin())) + throw runtime_error("Domain::partiallyApply: unsatisfiable"); + return boost::make_shared(Dk); +} + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 5acc5a08f..ae137ca33 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -7,111 +7,107 @@ #pragma once -#include #include +#include namespace gtsam { - /** - * Domain restriction constraint +/** + * The Domain class represents a constraint that restricts the possible values a + * particular variable, with given key, can take on. + */ +class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { + size_t cardinality_; /// Cardinality + std::set values_; /// allowed values + + public: + typedef boost::shared_ptr shared_ptr; + + // Constructor on Discrete Key initializes an "all-allowed" domain + Domain(const DiscreteKey& dkey) + : Constraint(dkey.first), cardinality_(dkey.second) { + for (size_t v = 0; v < cardinality_; v++) values_.insert(v); + } + + // Constructor on Discrete Key with single allowed value + // Consider SingleValue constraint + Domain(const DiscreteKey& dkey, size_t v) + : Constraint(dkey.first), cardinality_(dkey.second) { + values_.insert(v); + } + + /// The one key + Key key() const { return keys_[0]; } + + // The associated discrete key + DiscreteKey discreteKey() const { return DiscreteKey(key(), cardinality_); } + + /// Insert a value, non const :-( + void insert(size_t value) { values_.insert(value); } + + /// Erase a value, non const :-( + void erase(size_t value) { values_.erase(value); } + + size_t nrValues() const { return values_.size(); } + + bool isSingleton() const { return nrValues() == 1; } + + size_t firstValue() const { return *values_.begin(); } + + // print + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) + return false; + else { + const Domain& f(static_cast(other)); + return (cardinality_ == f.cardinality_) && (values_ == f.values_); + } + } + + // Return concise string representation, mostly to debug arc consistency. + // Converts from base 0 to base1. + std::string base1Str() const; + + // Check whether domain cotains a specific value. + bool contains(size_t value) const { return values_.count(value) > 0; } + + /// Calculate value + double operator()(const Values& values) const override; + + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + /* + * Ensure Arc-consistency by checking every possible value of domain j. + * @param j domain to be checked + * @param (in/out) domains all domains, but only domains->at(j) will be + * checked. + * @return true if domains->at(j) was changed, false otherwise. */ - class GTSAM_UNSTABLE_EXPORT Domain: public Constraint { + bool ensureArcConsistency(Key j, Domains* domains) const override; - size_t cardinality_; /// Cardinality - std::set values_; /// allowed values + /** + * Check for a value in domain that does not occur in any other connected + * domain. If found, return a a new singleton domain... + * Called in AllDiff::ensureArcConsistency + * @param keys connected domains through alldiff + * @param keys other domains + */ + boost::optional checkAllDiff(const KeyVector keys, + const Domains& domains) const; - public: + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values& values) const override; - typedef boost::shared_ptr shared_ptr; + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply(const Domains& domains) const override; +}; - // Constructor on Discrete Key initializes an "all-allowed" domain - Domain(const DiscreteKey& dkey) : - Constraint(dkey.first), cardinality_(dkey.second) { - for (size_t v = 0; v < cardinality_; v++) - values_.insert(v); - } - - // Constructor on Discrete Key with single allowed value - // Consider SingleValue constraint - Domain(const DiscreteKey& dkey, size_t v) : - Constraint(dkey.first), cardinality_(dkey.second) { - values_.insert(v); - } - - /// Constructor - Domain(const Domain& other) : - Constraint(other.keys_[0]), values_(other.values_) { - } - - /// insert a value, non const :-( - void insert(size_t value) { - values_.insert(value); - } - - /// erase a value, non const :-( - void erase(size_t value) { - values_.erase(value); - } - - size_t nrValues() const { - return values_.size(); - } - - bool isSingleton() const { - return nrValues() == 1; - } - - size_t firstValue() const { - return *values_.begin(); - } - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - 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; - } - - /// Calculate value - double operator()(const Values& values) const override; - - /// Convert into a decisiontree - DecisionTreeFactor toDecisionTreeFactor() const override; - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains - */ - bool ensureArcConsistency(size_t j, std::vector& domains) const override; - - /** - * Check for a value in domain that does not occur in any other connected domain. - * If found, we make this a singleton... Called in AllDiff::ensureArcConsistency - * @param keys connected domains through alldiff - */ - bool checkAllDiff(const KeyVector keys, std::vector& domains); - - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values& values) const override; - - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector& domains) const override; - }; - -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index 3273778c4..6210f8037 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -5,298 +5,285 @@ * @author Frank Dellaert */ -#include -#include #include #include +#include +#include #include - +#include #include #include -#include namespace gtsam { - using namespace std; +using namespace std; - Scheduler::Scheduler(size_t maxNrStudents, const string& filename): - maxNrStudents_(maxNrStudents) - { - typedef boost::tokenizer > Tokenizer; +Scheduler::Scheduler(size_t maxNrStudents, const string& filename) + : maxNrStudents_(maxNrStudents) { + typedef boost::tokenizer > Tokenizer; - // open file - ifstream is(filename.c_str()); - if (!is) { - cerr << "Scheduler: could not open file " << filename << endl; - throw runtime_error("Scheduler: could not open file " + filename); - } - - string line; // buffer - - // process first line with faculty - if (getline(is, line, '\r')) { - Tokenizer tok(line); - Tokenizer::iterator it = tok.begin(); - for (++it; it != tok.end(); ++it) - addFaculty(*it); - } - - // for all remaining lines - size_t count = 0; - while (getline(is, line, '\r')) { - if (count++ > 100) throw runtime_error("reached 100 lines, exiting"); - Tokenizer tok(line); - Tokenizer::iterator it = tok.begin(); - addSlot(*it++); // add slot - // add availability - for (; it != tok.end(); ++it) - available_ += (it->empty()) ? "0 " : "1 "; - available_ += '\n'; - } - } // constructor - - /** addStudent has to be called after adding slots and faculty */ - void Scheduler::addStudent(const string& studentName, - const string& area1, const string& area2, - const string& area3, const string& advisor) { - assert(nrStudents() area) const { - return area ? students_[s].keys_[*area] : students_[s].key_; + // open file + ifstream is(filename.c_str()); + if (!is) { + cerr << "Scheduler: could not open file " << filename << endl; + throw runtime_error("Scheduler: could not open file " + filename); } - const string& Scheduler::studentName(size_t i) const { - assert(i 100) throw runtime_error("reached 100 lines, exiting"); + Tokenizer tok(line); + Tokenizer::iterator it = tok.begin(); + addSlot(*it++); // add slot + // add availability + for (; it != tok.end(); ++it) available_ += (it->empty()) ? "0 " : "1 "; + available_ += '\n'; + } +} // constructor + +/** addStudent has to be called after adding slots and faculty */ +void Scheduler::addStudent(const string& studentName, const string& area1, + const string& area2, const string& area3, + const string& advisor) { + assert(nrStudents() < maxNrStudents_); + assert(facultyInArea_.count(area1)); + assert(facultyInArea_.count(area2)); + assert(facultyInArea_.count(area3)); + size_t advisorIndex = facultyIndex_[advisor]; + Student student(nrFaculty(), advisorIndex); + student.name_ = studentName; + // We fix the ordering by assigning a higher index to the student + // and numbering the areas lower + Key j = 3 * maxNrStudents_ + nrStudents(); + student.key_ = DiscreteKey(j, nrTimeSlots()); + Key base = 3 * nrStudents(); + student.keys_[0] = DiscreteKey(base + 0, nrFaculty()); + student.keys_[1] = DiscreteKey(base + 1, nrFaculty()); + student.keys_[2] = DiscreteKey(base + 2, nrFaculty()); + student.areaName_[0] = area1; + student.areaName_[1] = area2; + student.areaName_[2] = area3; + students_.push_back(student); +} + +/** get key for student and area, 0 is time slot itself */ +const DiscreteKey& Scheduler::key(size_t s, + boost::optional area) const { + return area ? students_[s].keys_[*area] : students_[s].key_; +} + +const string& Scheduler::studentName(size_t i) const { + assert(i < nrStudents()); + return students_[i].name_; +} + +const DiscreteKey& Scheduler::studentKey(size_t i) const { + assert(i < nrStudents()); + return students_[i].key_; +} + +const string& Scheduler::studentArea(size_t i, size_t area) const { + assert(i < nrStudents()); + return students_[i].areaName_[area]; +} + +/** Add student-specific constraints to the graph */ +void Scheduler::addStudentSpecificConstraints(size_t i, + boost::optional slot) { + bool debug = ISDEBUG("Scheduler::buildGraph"); + + assert(i < nrStudents()); + const Student& s = students_[i]; + + if (!slot && !slotsAvailable_.empty()) { + if (debug) cout << "Adding availability of slots" << endl; + assert(slotsAvailable_.size() == s.key_.second); + CSP::add(s.key_, slotsAvailable_); } - const string& Scheduler::studentArea(size_t i, size_t area) const { - assert(i slot) { - bool debug = ISDEBUG("Scheduler::buildGraph"); + if (debug) cout << "Area constraints " << areaName << endl; + assert(facultyInArea_[areaName].size() == areaKey.second); + CSP::add(areaKey, facultyInArea_[areaName]); - assert(iat(j); - cout << studentName(s) << " slot: " << slotName_[slot] << endl; - Key base = 3*s; - for (size_t area = 0; area < 3; area++) { - size_t faculty = assignment->at(base+area); - cout << setw(12) << studentArea(s,area) << ": " << facultyName_[faculty] - << endl; - } - cout << endl; - } } +} // buildGraph - /** Special print for single-student case */ - void Scheduler::printSpecial(sharedValues assignment) const { - Values::const_iterator it = assignment->begin(); - for (size_t area = 0; area < 3; area++, it++) { - size_t f = it->second; - cout << setw(12) << studentArea(0,area) << ": " << facultyName_[f] << endl; +/** print */ +void Scheduler::print(const string& s, const KeyFormatter& formatter) const { + cout << s << " Faculty:" << endl; + for (const string& name : facultyName_) cout << name << '\n'; + cout << endl; + + cout << s << " Slots:\n"; + size_t i = 0; + for (const string& name : slotName_) cout << i++ << " " << name << endl; + cout << endl; + + cout << "Availability:\n" << available_ << '\n'; + + cout << s << " Area constraints:\n"; + for (const FacultyInArea::value_type& it : facultyInArea_) { + cout << setw(12) << it.first << ": "; + for (double v : it.second) cout << v << " "; + cout << '\n'; + } + cout << endl; + + cout << s << " Students:\n"; + for (const Student& student : students_) student.print(); + cout << endl; + + CSP::print(s + " Factor graph"); + cout << endl; +} // print + +/** Print readable form of assignment */ +void Scheduler::printAssignment(const Values& assignment) const { + // Not intended to be general! Assumes very particular ordering ! + cout << endl; + for (size_t s = 0; s < nrStudents(); s++) { + Key j = 3 * maxNrStudents_ + s; + size_t slot = assignment.at(j); + cout << studentName(s) << " slot: " << slotName_[slot] << endl; + Key base = 3 * s; + for (size_t area = 0; area < 3; area++) { + size_t faculty = assignment.at(base + area); + cout << setw(12) << studentArea(s, area) << ": " << facultyName_[faculty] + << endl; } cout << endl; } +} - /** Accumulate faculty stats */ - void Scheduler::accumulateStats(sharedValues assignment, vector< - size_t>& stats) const { - for (size_t s = 0; s < nrStudents(); s++) { - Key base = 3*s; - for (size_t area = 0; area < 3; area++) { - size_t f = assignment->at(base+area); - assert(fsecond; + cout << setw(12) << studentArea(0, area) << ": " << facultyName_[f] << endl; + } + cout << endl; +} + +/** Accumulate faculty stats */ +void Scheduler::accumulateStats(const Values& assignment, + vector& stats) const { + for (size_t s = 0; s < nrStudents(); s++) { + Key base = 3 * s; + for (size_t area = 0; area < 3; area++) { + size_t f = assignment.at(base + area); + assert(f < stats.size()); + stats[f]++; + } // area + } // s +} + +/** Eliminate, return a Bayes net */ +DiscreteBayesNet::shared_ptr Scheduler::eliminate() const { + gttic(my_eliminate); + // TODO: fix this!! + size_t maxKey = keys().size(); + Ordering defaultKeyOrdering; + for (size_t i = 0; i < maxKey; ++i) defaultKeyOrdering += Key(i); + DiscreteBayesNet::shared_ptr chordal = + this->eliminateSequential(defaultKeyOrdering); + gttoc(my_eliminate); + return chordal; +} + +/** Find the best total assignment - can be expensive */ +Scheduler::Values Scheduler::optimalAssignment() const { + DiscreteBayesNet::shared_ptr chordal = eliminate(); + + if (ISDEBUG("Scheduler::optimalAssignment")) { + DiscreteBayesNet::const_iterator it = chordal->end() - 1; + const Student& student = students_.front(); + cout << endl; + (*it)->print(student.name_); } - /** Eliminate, return a Bayes net */ - DiscreteBayesNet::shared_ptr Scheduler::eliminate() const { - gttic(my_eliminate); - // TODO: fix this!! - size_t maxKey = keys().size(); - Ordering defaultKeyOrdering; - for (size_t i = 0; ieliminateSequential(defaultKeyOrdering); - gttoc(my_eliminate); - return chordal; - } + gttic(my_optimize); + Values mpe = chordal->optimize(); + gttoc(my_optimize); + return mpe; +} - /** Find the best total assignment - can be expensive */ - Scheduler::sharedValues Scheduler::optimalAssignment() const { - DiscreteBayesNet::shared_ptr chordal = eliminate(); - - if (ISDEBUG("Scheduler::optimalAssignment")) { - DiscreteBayesNet::const_iterator it = chordal->end()-1; - const Student & student = students_.front(); - cout << endl; - (*it)->print(student.name_); - } - - gttic(my_optimize); - sharedValues mpe = chordal->optimize(); - gttoc(my_optimize); - return mpe; - } - - /** find the assignment of students to slots with most possible committees */ - Scheduler::sharedValues Scheduler::bestSchedule() const { - sharedValues best; - throw runtime_error("bestSchedule not implemented"); - return best; - } - - /** find the corresponding most desirable committee assignment */ - Scheduler::sharedValues Scheduler::bestAssignment( - sharedValues bestSchedule) const { - sharedValues best; - throw runtime_error("bestAssignment not implemented"); - return best; - } - -} // gtsam +/** find the assignment of students to slots with most possible committees */ +Scheduler::Values Scheduler::bestSchedule() const { + Values best; + throw runtime_error("bestSchedule not implemented"); + return best; +} +/** find the corresponding most desirable committee assignment */ +Scheduler::Values Scheduler::bestAssignment(const Values& bestSchedule) const { + Values best; + throw runtime_error("bestAssignment not implemented"); + return best; +} +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Scheduler.h b/gtsam_unstable/discrete/Scheduler.h index 6faf9956f..08e866efd 100644 --- a/gtsam_unstable/discrete/Scheduler.h +++ b/gtsam_unstable/discrete/Scheduler.h @@ -11,165 +11,150 @@ namespace gtsam { +/** + * Scheduler class + * Creates one variable for each student, and three variables for each + * of the student's areas, for a total of 4*nrStudents variables. + * The "student" variable will determine when the student takes the qual. + * The "area" variables determine which faculty are on his/her committee. + */ +class GTSAM_UNSTABLE_EXPORT Scheduler : public CSP { + private: + /** Internal data structure for students */ + struct Student { + std::string name_; + DiscreteKey key_; // key for student + std::vector keys_; // key for areas + std::vector areaName_; + std::vector advisor_; + Student(size_t nrFaculty, size_t advisorIndex) + : keys_(3), areaName_(3), advisor_(nrFaculty, 1.0) { + advisor_[advisorIndex] = 0.0; + } + void print() const { + using std::cout; + cout << name_ << ": "; + for (size_t area = 0; area < 3; area++) cout << areaName_[area] << " "; + cout << std::endl; + } + }; + + /** Maximum number of students */ + size_t maxNrStudents_; + + /** discrete keys, indexed by student and area index */ + std::vector students_; + + /** faculty identifiers */ + std::map facultyIndex_; + std::vector facultyName_, slotName_, areaName_; + + /** area constraints */ + typedef std::map > FacultyInArea; + FacultyInArea facultyInArea_; + + /** nrTimeSlots * nrFaculty availability constraints */ + std::string available_; + + /** which slots are good */ + std::vector slotsAvailable_; + + public: /** - * Scheduler class - * Creates one variable for each student, and three variables for each - * of the student's areas, for a total of 4*nrStudents variables. - * The "student" variable will determine when the student takes the qual. - * The "area" variables determine which faculty are on his/her committee. + * Constructor + * We need to know the number of students in advance for ordering keys. + * then add faculty, slots, areas, availability, students, in that order */ - class GTSAM_UNSTABLE_EXPORT Scheduler : public CSP { + Scheduler(size_t maxNrStudents) : maxNrStudents_(maxNrStudents) {} - private: + /// Destructor + virtual ~Scheduler() {} - /** Internal data structure for students */ - struct Student { - std::string name_; - DiscreteKey key_; // key for student - std::vector keys_; // key for areas - std::vector areaName_; - std::vector advisor_; - Student(size_t nrFaculty, size_t advisorIndex) : - keys_(3), areaName_(3), advisor_(nrFaculty, 1.0) { - advisor_[advisorIndex] = 0.0; - } - void print() const { - using std::cout; - cout << name_ << ": "; - for (size_t area = 0; area < 3; area++) - cout << areaName_[area] << " "; - cout << std::endl; - } - }; + void addFaculty(const std::string& facultyName) { + facultyIndex_[facultyName] = nrFaculty(); + facultyName_.push_back(facultyName); + } - /** Maximum number of students */ - size_t maxNrStudents_; + size_t nrFaculty() const { return facultyName_.size(); } - /** discrete keys, indexed by student and area index */ - std::vector students_; + /** boolean std::string of nrTimeSlots * nrFaculty */ + void setAvailability(const std::string& available) { available_ = available; } - /** faculty identifiers */ - std::map facultyIndex_; - std::vector facultyName_, slotName_, areaName_; + void addSlot(const std::string& slotName) { slotName_.push_back(slotName); } - /** area constraints */ - typedef std::map > FacultyInArea; - FacultyInArea facultyInArea_; + size_t nrTimeSlots() const { return slotName_.size(); } - /** nrTimeSlots * nrFaculty availability constraints */ - std::string available_; + const std::string& slotName(size_t s) const { return slotName_[s]; } - /** which slots are good */ - std::vector slotsAvailable_; + /** slots available, boolean */ + void setSlotsAvailable(const std::vector& slotsAvailable) { + slotsAvailable_ = slotsAvailable; + } - public: + void addArea(const std::string& facultyName, const std::string& areaName) { + areaName_.push_back(areaName); + std::vector& table = + facultyInArea_[areaName]; // will create if needed + if (table.empty()) table.resize(nrFaculty(), 0); + table[facultyIndex_[facultyName]] = 1; + } - /** - * Constructor - * We need to know the number of students in advance for ordering keys. - * then add faculty, slots, areas, availability, students, in that order - */ - Scheduler(size_t maxNrStudents) : maxNrStudents_(maxNrStudents) {} + /** + * Constructor that reads in faculty, slots, availibility. + * Still need to add areas and students after this + */ + Scheduler(size_t maxNrStudents, const std::string& filename); - /// Destructor - virtual ~Scheduler() {} + /** get key for student and area, 0 is time slot itself */ + const DiscreteKey& key(size_t s, + boost::optional area = boost::none) const; - void addFaculty(const std::string& facultyName) { - facultyIndex_[facultyName] = nrFaculty(); - facultyName_.push_back(facultyName); - } + /** addStudent has to be called after adding slots and faculty */ + void addStudent(const std::string& studentName, const std::string& area1, + const std::string& area2, const std::string& area3, + const std::string& advisor); - size_t nrFaculty() const { - return facultyName_.size(); - } + /// current number of students + size_t nrStudents() const { return students_.size(); } - /** boolean std::string of nrTimeSlots * nrFaculty */ - void setAvailability(const std::string& available) { - available_ = available; - } + const std::string& studentName(size_t i) const; + const DiscreteKey& studentKey(size_t i) const; + const std::string& studentArea(size_t i, size_t area) const; - void addSlot(const std::string& slotName) { - slotName_.push_back(slotName); - } + /** Add student-specific constraints to the graph */ + void addStudentSpecificConstraints( + size_t i, boost::optional slot = boost::none); - size_t nrTimeSlots() const { - return slotName_.size(); - } + /** Main routine that builds factor graph */ + void buildGraph(size_t mutexBound = 7); - const std::string& slotName(size_t s) const { - return slotName_[s]; - } + /** print */ + void print( + const std::string& s = "Scheduler", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; - /** slots available, boolean */ - void setSlotsAvailable(const std::vector& slotsAvailable) { - slotsAvailable_ = slotsAvailable; - } + /** Print readable form of assignment */ + void printAssignment(const Values& assignment) const; - void addArea(const std::string& facultyName, const std::string& areaName) { - areaName_.push_back(areaName); - std::vector& table = facultyInArea_[areaName]; // will create if needed - if (table.empty()) table.resize(nrFaculty(), 0); - table[facultyIndex_[facultyName]] = 1; - } + /** Special print for single-student case */ + void printSpecial(const Values& assignment) const; - /** - * Constructor that reads in faculty, slots, availibility. - * Still need to add areas and students after this - */ - Scheduler(size_t maxNrStudents, const std::string& filename); + /** Accumulate faculty stats */ + void accumulateStats(const Values& assignment, + std::vector& stats) const; - /** get key for student and area, 0 is time slot itself */ - const DiscreteKey& key(size_t s, boost::optional area = boost::none) const; + /** Eliminate, return a Bayes net */ + DiscreteBayesNet::shared_ptr eliminate() const; - /** addStudent has to be called after adding slots and faculty */ - void addStudent(const std::string& studentName, const std::string& area1, - const std::string& area2, const std::string& area3, - const std::string& advisor); + /** Find the best total assignment - can be expensive */ + Values optimalAssignment() const; - /// current number of students - size_t nrStudents() const { - return students_.size(); - } + /** find the assignment of students to slots with most possible committees */ + Values bestSchedule() const; - const std::string& studentName(size_t i) const; - const DiscreteKey& studentKey(size_t i) const; - const std::string& studentArea(size_t i, size_t area) const; - - /** Add student-specific constraints to the graph */ - void addStudentSpecificConstraints(size_t i, boost::optional slot = boost::none); - - /** Main routine that builds factor graph */ - void buildGraph(size_t mutexBound = 7); - - /** print */ - void print( - const std::string& s = "Scheduler", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /** Print readable form of assignment */ - void printAssignment(sharedValues assignment) const; - - /** Special print for single-student case */ - void printSpecial(sharedValues assignment) const; - - /** Accumulate faculty stats */ - void accumulateStats(sharedValues assignment, - std::vector& stats) const; - - /** Eliminate, return a Bayes net */ - DiscreteBayesNet::shared_ptr eliminate() const; - - /** Find the best total assignment - can be expensive */ - sharedValues optimalAssignment() const; - - /** find the assignment of students to slots with most possible committees */ - sharedValues bestSchedule() const; - - /** find the corresponding most desirable committee assignment */ - sharedValues bestAssignment(sharedValues bestSchedule) const; - - }; // Scheduler - -} // gtsam + /** find the corresponding most desirable committee assignment */ + Values bestAssignment(const Values& bestSchedule) const; +}; // Scheduler +} // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 6324f14cd..162e21512 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -5,75 +5,73 @@ * @author Frank Dellaert */ -#include -#include -#include #include +#include +#include +#include + #include namespace gtsam { - using namespace std; - - /* ************************************************************************* */ - void SingleValue::print(const string& s, - const KeyFormatter& formatter) const { - cout << s << "SingleValue on " << "j=" << formatter(keys_[0]) - << " with value " << value_ << endl; - } - - /* ************************************************************************* */ - double SingleValue::operator()(const Values& values) const { - return (double) (values.at(keys_[0]) == value_); - } - - /* ************************************************************************* */ - DecisionTreeFactor SingleValue::toDecisionTreeFactor() const { - DiscreteKeys keys; - keys += DiscreteKey(keys_[0],cardinality_); - vector table; - for (size_t i1 = 0; i1 < cardinality_; i1++) - table.push_back(i1 == value_); - DecisionTreeFactor converted(keys, table); - return converted; - } - - /* ************************************************************************* */ - DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* ************************************************************************* */ - bool SingleValue::ensureArcConsistency(size_t j, - vector& domains) const { - if (j != keys_[0]) throw invalid_argument( - "SingleValue check on wrong domain"); - Domain& D = domains[j]; - if (D.isSingleton()) { - if (D.firstValue() != value_) throw runtime_error("Unsatisfiable"); - return false; - } - D = Domain(discreteKey(),value_); - return true; - } - - /* ************************************************************************* */ - Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const { - Values::const_iterator it = values.find(keys_[0]); - if (it != values.end() && it->second != value_) throw runtime_error( - "SingleValue::partiallyApply: unsatisfiable"); - return boost::make_shared < SingleValue > (keys_[0], cardinality_, value_); - } - - /* ************************************************************************* */ - Constraint::shared_ptr SingleValue::partiallyApply( - const vector& domains) const { - const Domain& Dk = domains[keys_[0]]; - if (Dk.isSingleton() && !Dk.contains(value_)) throw runtime_error( - "SingleValue::partiallyApply: unsatisfiable"); - return boost::make_shared < SingleValue > (discreteKey(), value_); - } +using namespace std; /* ************************************************************************* */ -} // namespace gtsam +void SingleValue::print(const string& s, const KeyFormatter& formatter) const { + cout << s << "SingleValue on " + << "j=" << formatter(keys_[0]) << " with value " << value_ << endl; +} + +/* ************************************************************************* */ +double SingleValue::operator()(const Values& values) const { + return (double)(values.at(keys_[0]) == value_); +} + +/* ************************************************************************* */ +DecisionTreeFactor SingleValue::toDecisionTreeFactor() const { + DiscreteKeys keys; + keys += DiscreteKey(keys_[0], cardinality_); + vector table; + for (size_t i1 = 0; i1 < cardinality_; i1++) table.push_back(i1 == value_); + DecisionTreeFactor converted(keys, table); + return converted; +} + +/* ************************************************************************* */ +DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; +} + +/* ************************************************************************* */ +bool SingleValue::ensureArcConsistency(Key j, Domains* domains) const { + if (j != keys_[0]) + throw invalid_argument("SingleValue check on wrong domain"); + Domain& D = domains->at(j); + if (D.isSingleton()) { + if (D.firstValue() != value_) throw runtime_error("Unsatisfiable"); + return false; + } + D = Domain(discreteKey(), value_); + return true; +} + +/* ************************************************************************* */ +Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const { + Values::const_iterator it = values.find(keys_[0]); + if (it != values.end() && it->second != value_) + throw runtime_error("SingleValue::partiallyApply: unsatisfiable"); + return boost::make_shared(keys_[0], cardinality_, value_); +} + +/* ************************************************************************* */ +Constraint::shared_ptr SingleValue::partiallyApply( + const Domains& domains) const { + const Domain& Dk = domains.at(keys_[0]); + if (Dk.isSingleton() && !Dk.contains(value_)) + throw runtime_error("SingleValue::partiallyApply: unsatisfiable"); + return boost::make_shared(discreteKey(), value_); +} + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index c4d2addec..d826093df 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -7,76 +7,71 @@ #pragma once -#include #include +#include namespace gtsam { - /** - * SingleValue constraint +/** + * SingleValue constraint: ensures a variable takes on a certain value. + * This could of course also be implemented by changing its `Domain`. + */ +class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { + size_t cardinality_; /// < Number of values + size_t value_; ///< allowed value + + DiscreteKey discreteKey() const { + return DiscreteKey(keys_[0], cardinality_); + } + + public: + typedef boost::shared_ptr shared_ptr; + + /// Construct from key, cardinality, and given value. + SingleValue(Key key, size_t n, size_t value) + : Constraint(key), cardinality_(n), value_(value) {} + + /// Construct from DiscreteKey and given value. + SingleValue(const DiscreteKey& dkey, size_t value) + : Constraint(dkey.first), cardinality_(dkey.second), value_(value) {} + + // print + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) + return false; + else { + const SingleValue& f(static_cast(other)); + return (cardinality_ == f.cardinality_) && (value_ == f.value_); + } + } + + /// Calculate value + double operator()(const Values& values) const override; + + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + /* + * Ensure Arc-consistency: just sets domain[j] to {value_}. + * @param j domain to be checked + * @param (in/out) domains all domains, but only domains->at(j) will be checked. + * @return true if domains->at(j) was changed, false otherwise. */ - class GTSAM_UNSTABLE_EXPORT SingleValue: public Constraint { + bool ensureArcConsistency(Key j, Domains* domains) const override; - /// Number of values - size_t cardinality_; + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values& values) const override; - /// allowed value - size_t value_; + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const Domains& domains) const override; +}; - DiscreteKey discreteKey() const { - return DiscreteKey(keys_[0],cardinality_); - } - - public: - - typedef boost::shared_ptr shared_ptr; - - /// Constructor - SingleValue(Key key, size_t n, size_t value) : - Constraint(key), cardinality_(n), value_(value) { - } - - /// Constructor - SingleValue(const DiscreteKey& dkey, size_t value) : - Constraint(dkey.first), cardinality_(dkey.second), value_(value) { - } - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if(!dynamic_cast(&other)) - return false; - else { - const SingleValue& f(static_cast(other)); - return (cardinality_==f.cardinality_) && (value_==f.value_); - } - } - - /// Calculate value - double operator()(const Values& values) const override; - - /// Convert into a decisiontree - DecisionTreeFactor toDecisionTreeFactor() const override; - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains - */ - bool ensureArcConsistency(size_t j, std::vector& domains) const override; - - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values& values) const override; - - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector& domains) const override; - }; - -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/examples/schedulingExample.cpp b/gtsam_unstable/discrete/examples/schedulingExample.cpp index e9f63b2d8..3f270e45d 100644 --- a/gtsam_unstable/discrete/examples/schedulingExample.cpp +++ b/gtsam_unstable/discrete/examples/schedulingExample.cpp @@ -122,7 +122,7 @@ void runLargeExample() { // SETDEBUG("timing-verbose", true); SETDEBUG("DiscreteConditional::DiscreteConditional", true); gttic(large); - DiscreteFactor::sharedValues MPE = scheduler.optimalAssignment(); + auto MPE = scheduler.optimalAssignment(); gttoc(large); tictoc_finishedIteration(); tictoc_print(); @@ -225,7 +225,7 @@ void sampleSolutions() { // now, sample schedules for (size_t n = 0; n < 500; n++) { vector stats(19, 0); - vector samples; + vector samples; for (size_t i = 0; i < 7; i++) { samples.push_back(samplers[i]->sample()); schedulers[i].accumulateStats(samples[i], stats); @@ -331,7 +331,7 @@ void accomodateStudent() { // sample schedules for (size_t n = 0; n < 10; n++) { - Scheduler::sharedValues sample0 = chordal->sample(); + auto sample0 = chordal->sample(); scheduler.printAssignment(sample0); } } diff --git a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp index 1fc4a1459..5ed5766d5 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp @@ -129,7 +129,7 @@ void runLargeExample() { tictoc_finishedIteration(); tictoc_print(); for (size_t i=0;i<100;i++) { - DiscreteFactor::sharedValues assignment = chordal->sample(); + auto assignment = chordal->sample(); vector stats(scheduler.nrFaculty()); scheduler.accumulateStats(assignment, stats); size_t max = *max_element(stats.begin(), stats.end()); @@ -143,7 +143,7 @@ void runLargeExample() { } #else gttic(large); - DiscreteFactor::sharedValues MPE = scheduler.optimalAssignment(); + auto MPE = scheduler.optimalAssignment(); gttoc(large); tictoc_finishedIteration(); tictoc_print(); @@ -234,7 +234,7 @@ void sampleSolutions() { // now, sample schedules for (size_t n = 0; n < 500; n++) { vector stats(19, 0); - vector samples; + vector samples; for (size_t i = 0; i < NRSTUDENTS; i++) { samples.push_back(samplers[i]->sample()); schedulers[i].accumulateStats(samples[i], stats); diff --git a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp index 95b64f289..2da0eb9b1 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp @@ -153,7 +153,7 @@ void runLargeExample() { tictoc_finishedIteration(); tictoc_print(); for (size_t i=0;i<100;i++) { - DiscreteFactor::sharedValues assignment = sample(*chordal); + auto assignment = sample(*chordal); vector stats(scheduler.nrFaculty()); scheduler.accumulateStats(assignment, stats); size_t max = *max_element(stats.begin(), stats.end()); @@ -167,7 +167,7 @@ void runLargeExample() { } #else gttic(large); - DiscreteFactor::sharedValues MPE = scheduler.optimalAssignment(); + auto MPE = scheduler.optimalAssignment(); gttoc(large); tictoc_finishedIteration(); tictoc_print(); @@ -259,7 +259,7 @@ void sampleSolutions() { // now, sample schedules for (size_t n = 0; n < 10000; n++) { vector stats(nrFaculty, 0); - vector samples; + vector samples; for (size_t i = 0; i < NRSTUDENTS; i++) { samples.push_back(samplers[i]->sample()); schedulers[i].accumulateStats(samples[i], stats); diff --git a/gtsam_unstable/discrete/tests/testCSP.cpp b/gtsam_unstable/discrete/tests/testCSP.cpp index 3dd493b1b..4a3b22561 100644 --- a/gtsam_unstable/discrete/tests/testCSP.cpp +++ b/gtsam_unstable/discrete/tests/testCSP.cpp @@ -7,49 +7,109 @@ #include #include + #include using boost::assign::insert; #include -#include + #include +#include using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST_UNSAFE( BinaryAllDif, allInOne) -{ - // Create keys and ordering +TEST(CSP, SingleValue) { + // Create keys for Idaho, Arizona, and Utah, allowing two colors for each: + size_t nrColors = 3; + DiscreteKey ID(0, nrColors), AZ(1, nrColors), UT(2, nrColors); + + // Check that a single value is equal to a decision stump with only one "1": + SingleValue singleValue(AZ, 2); + DecisionTreeFactor f1(AZ, "0 0 1"); + EXPECT(assert_equal(f1, singleValue.toDecisionTreeFactor())); + + // Create domains + Domains domains; + domains.emplace(0, Domain(ID)); + domains.emplace(1, Domain(AZ)); + domains.emplace(2, Domain(UT)); + + // Ensure arc-consistency: just wipes out values in AZ domain: + EXPECT(singleValue.ensureArcConsistency(1, &domains)); + LONGS_EQUAL(3, domains.at(0).nrValues()); + LONGS_EQUAL(1, domains.at(1).nrValues()); + LONGS_EQUAL(3, domains.at(2).nrValues()); +} + +/* ************************************************************************* */ +TEST(CSP, BinaryAllDif) { + // Create keys for Idaho, Arizona, and Utah, allowing 2 colors for each: size_t nrColors = 2; -// DiscreteKey ID("Idaho", nrColors), UT("Utah", nrColors), AZ("Arizona", nrColors); - DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); + DiscreteKey ID(0, nrColors), AZ(1, nrColors), UT(2, nrColors); // Check construction and conversion BinaryAllDiff c1(ID, UT); DecisionTreeFactor f1(ID & UT, "0 1 1 0"); - EXPECT(assert_equal(f1,c1.toDecisionTreeFactor())); + EXPECT(assert_equal(f1, c1.toDecisionTreeFactor())); // Check construction and conversion BinaryAllDiff c2(UT, AZ); DecisionTreeFactor f2(UT & AZ, "0 1 1 0"); - EXPECT(assert_equal(f2,c2.toDecisionTreeFactor())); + EXPECT(assert_equal(f2, c2.toDecisionTreeFactor())); - DecisionTreeFactor f3 = f1*f2; - EXPECT(assert_equal(f3,c1*f2)); - EXPECT(assert_equal(f3,c2*f1)); + // Check multiplication of factors with constraint: + DecisionTreeFactor f3 = f1 * f2; + EXPECT(assert_equal(f3, c1 * f2)); + EXPECT(assert_equal(f3, c2 * f1)); } /* ************************************************************************* */ -TEST_UNSAFE( CSP, allInOne) -{ - // Create keys and ordering +TEST(CSP, AllDiff) { + // Create keys for Idaho, Arizona, and Utah, allowing two colors for each: + size_t nrColors = 3; + DiscreteKey ID(0, nrColors), AZ(1, nrColors), UT(2, nrColors); + + // Check construction and conversion + vector dkeys{ID, UT, AZ}; + AllDiff alldiff(dkeys); + DecisionTreeFactor actual = alldiff.toDecisionTreeFactor(); + // GTSAM_PRINT(actual); + actual.dot("actual"); + DecisionTreeFactor f2( + ID & AZ & UT, + "0 0 0 0 0 1 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 1 0 0 0 0 0"); + EXPECT(assert_equal(f2, actual)); + + // Create domains. + Domains domains; + domains.emplace(0, Domain(ID)); + domains.emplace(1, Domain(AZ)); + domains.emplace(2, Domain(UT)); + + // First constrict AZ domain: + SingleValue singleValue(AZ, 2); + EXPECT(singleValue.ensureArcConsistency(1, &domains)); + + // Arc-consistency + EXPECT(alldiff.ensureArcConsistency(0, &domains)); + EXPECT(!alldiff.ensureArcConsistency(1, &domains)); + EXPECT(alldiff.ensureArcConsistency(2, &domains)); + LONGS_EQUAL(2, domains.at(0).nrValues()); + LONGS_EQUAL(1, domains.at(1).nrValues()); + LONGS_EQUAL(2, domains.at(2).nrValues()); +} + +/* ************************************************************************* */ +TEST(CSP, allInOne) { + // Create keys for Idaho, Arizona, and Utah, allowing 3 colors for each: size_t nrColors = 2; - DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); + DiscreteKey ID(0, nrColors), AZ(1, nrColors), UT(2, nrColors); // Create the CSP CSP csp; - csp.addAllDiff(ID,UT); - csp.addAllDiff(UT,AZ); + csp.addAllDiff(ID, UT); + csp.addAllDiff(UT, AZ); // Check an invalid combination, with ID==UT==AZ all same color DiscreteFactor::Values invalid; @@ -69,68 +129,66 @@ TEST_UNSAFE( CSP, allInOne) DecisionTreeFactor product = csp.product(); // product.dot("product"); DecisionTreeFactor expectedProduct(ID & AZ & UT, "0 1 0 0 0 0 1 0"); - EXPECT(assert_equal(expectedProduct,product)); + EXPECT(assert_equal(expectedProduct, product)); // Solve - CSP::sharedValues mpe = csp.optimalAssignment(); + auto mpe = csp.optimalAssignment(); CSP::Values expected; insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1); - EXPECT(assert_equal(expected,*mpe)); - EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); + EXPECT(assert_equal(expected, mpe)); + EXPECT_DOUBLES_EQUAL(1, csp(mpe), 1e-9); } /* ************************************************************************* */ -TEST_UNSAFE( CSP, WesternUS) -{ - // Create keys +TEST(CSP, WesternUS) { + // Create keys for all states in Western US, with 4 color possibilities. size_t nrColors = 4; - DiscreteKey - // Create ordering according to example in ND-CSP.lyx - WA(0, nrColors), OR(3, nrColors), CA(1, nrColors),NV(2, nrColors), - ID(8, nrColors), UT(9, nrColors), AZ(10, nrColors), - MT(4, nrColors), WY(5, nrColors), CO(7, nrColors), NM(6, nrColors); + DiscreteKey WA(0, nrColors), OR(3, nrColors), CA(1, nrColors), + NV(2, nrColors), ID(8, nrColors), UT(9, nrColors), AZ(10, nrColors), + MT(4, nrColors), WY(5, nrColors), CO(7, nrColors), NM(6, nrColors); // Create the CSP CSP csp; - csp.addAllDiff(WA,ID); - csp.addAllDiff(WA,OR); - csp.addAllDiff(OR,ID); - csp.addAllDiff(OR,CA); - csp.addAllDiff(OR,NV); - csp.addAllDiff(CA,NV); - csp.addAllDiff(CA,AZ); - csp.addAllDiff(ID,MT); - csp.addAllDiff(ID,WY); - csp.addAllDiff(ID,UT); - csp.addAllDiff(ID,NV); - csp.addAllDiff(NV,UT); - csp.addAllDiff(NV,AZ); - csp.addAllDiff(UT,WY); - csp.addAllDiff(UT,CO); - csp.addAllDiff(UT,NM); - csp.addAllDiff(UT,AZ); - csp.addAllDiff(AZ,CO); - csp.addAllDiff(AZ,NM); - csp.addAllDiff(MT,WY); - csp.addAllDiff(WY,CO); - csp.addAllDiff(CO,NM); + csp.addAllDiff(WA, ID); + csp.addAllDiff(WA, OR); + csp.addAllDiff(OR, ID); + csp.addAllDiff(OR, CA); + csp.addAllDiff(OR, NV); + csp.addAllDiff(CA, NV); + csp.addAllDiff(CA, AZ); + csp.addAllDiff(ID, MT); + csp.addAllDiff(ID, WY); + csp.addAllDiff(ID, UT); + csp.addAllDiff(ID, NV); + csp.addAllDiff(NV, UT); + csp.addAllDiff(NV, AZ); + csp.addAllDiff(UT, WY); + csp.addAllDiff(UT, CO); + csp.addAllDiff(UT, NM); + csp.addAllDiff(UT, AZ); + csp.addAllDiff(AZ, CO); + csp.addAllDiff(AZ, NM); + csp.addAllDiff(MT, WY); + csp.addAllDiff(WY, CO); + csp.addAllDiff(CO, NM); - // Solve + // Create ordering according to example in ND-CSP.lyx Ordering ordering; - ordering += Key(0),Key(1),Key(2),Key(3),Key(4),Key(5),Key(6),Key(7),Key(8),Key(9),Key(10); - CSP::sharedValues mpe = csp.optimalAssignment(ordering); - // GTSAM_PRINT(*mpe); + ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7), + Key(8), Key(9), Key(10); + // Solve using that ordering: + auto mpe = csp.optimalAssignment(ordering); + // GTSAM_PRINT(mpe); CSP::Values expected; - insert(expected) - (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); + insert(expected)(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??? - EXPECT(assert_equal(expected,*mpe)); - EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); + EXPECT(assert_equal(expected, mpe)); + EXPECT_DOUBLES_EQUAL(1, csp(mpe), 1e-9); // Write out the dual graph for hmetis #ifdef DUAL @@ -142,33 +200,17 @@ TEST_UNSAFE( CSP, WesternUS) } /* ************************************************************************* */ -TEST_UNSAFE( CSP, AllDiff) -{ - // Create keys and ordering +TEST(CSP, ArcConsistency) { + // Create keys for Idaho, Arizona, and Utah, allowing three colors for each: size_t nrColors = 3; - DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); + DiscreteKey ID(0, nrColors), AZ(1, nrColors), UT(2, nrColors); - // Create the CSP + // Create the CSP using just one all-diff constraint, plus constrain Arizona. CSP csp; - vector dkeys; - dkeys += ID,UT,AZ; + vector dkeys{ID, UT, AZ}; csp.addAllDiff(dkeys); - csp.addSingleValue(AZ,2); -// GTSAM_PRINT(csp); - - // Check construction and conversion - SingleValue s(AZ,2); - DecisionTreeFactor f1(AZ,"0 0 1"); - EXPECT(assert_equal(f1,s.toDecisionTreeFactor())); - - // Check construction and conversion - AllDiff alldiff(dkeys); - DecisionTreeFactor actual = alldiff.toDecisionTreeFactor(); -// GTSAM_PRINT(actual); -// actual.dot("actual"); - DecisionTreeFactor f2(ID & AZ & UT, - "0 0 0 0 0 1 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 1 0 0 0 0 0"); - EXPECT(assert_equal(f2,actual)); + csp.addSingleValue(AZ, 2); + // GTSAM_PRINT(csp); // Check an invalid combination, with ID==UT==AZ all same color DiscreteFactor::Values invalid; @@ -185,42 +227,47 @@ TEST_UNSAFE( CSP, AllDiff) EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9); // Solve - CSP::sharedValues mpe = csp.optimalAssignment(); + auto mpe = csp.optimalAssignment(); CSP::Values expected; insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2); - EXPECT(assert_equal(expected,*mpe)); - EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); + EXPECT(assert_equal(expected, mpe)); + EXPECT_DOUBLES_EQUAL(1, csp(mpe), 1e-9); - // Arc-consistency - vector domains; - domains += Domain(ID), Domain(AZ), Domain(UT); - SingleValue singleValue(AZ,2); - EXPECT(singleValue.ensureArcConsistency(1,domains)); - EXPECT(alldiff.ensureArcConsistency(0,domains)); - EXPECT(!alldiff.ensureArcConsistency(1,domains)); - EXPECT(alldiff.ensureArcConsistency(2,domains)); - LONGS_EQUAL(2,domains[0].nrValues()); - LONGS_EQUAL(1,domains[1].nrValues()); - LONGS_EQUAL(2,domains[2].nrValues()); + // ensure arc-consistency, i.e., narrow domains... + Domains domains; + domains.emplace(0, Domain(ID)); + domains.emplace(1, Domain(AZ)); + domains.emplace(2, Domain(UT)); + + SingleValue singleValue(AZ, 2); + AllDiff alldiff(dkeys); + EXPECT(singleValue.ensureArcConsistency(1, &domains)); + EXPECT(alldiff.ensureArcConsistency(0, &domains)); + EXPECT(!alldiff.ensureArcConsistency(1, &domains)); + EXPECT(alldiff.ensureArcConsistency(2, &domains)); + LONGS_EQUAL(2, domains.at(0).nrValues()); + LONGS_EQUAL(1, domains.at(1).nrValues()); + LONGS_EQUAL(2, domains.at(2).nrValues()); // Parial application, version 1 DiscreteFactor::Values known; known[AZ.first] = 2; DiscreteFactor::shared_ptr reduced1 = alldiff.partiallyApply(known); DecisionTreeFactor f3(ID & UT, "0 1 1 1 0 1 1 1 0"); - EXPECT(assert_equal(f3,reduced1->toDecisionTreeFactor())); + EXPECT(assert_equal(f3, reduced1->toDecisionTreeFactor())); DiscreteFactor::shared_ptr reduced2 = singleValue.partiallyApply(known); DecisionTreeFactor f4(AZ, "0 0 1"); - EXPECT(assert_equal(f4,reduced2->toDecisionTreeFactor())); + EXPECT(assert_equal(f4, reduced2->toDecisionTreeFactor())); // Parial application, version 2 DiscreteFactor::shared_ptr reduced3 = alldiff.partiallyApply(domains); - EXPECT(assert_equal(f3,reduced3->toDecisionTreeFactor())); + EXPECT(assert_equal(f3, reduced3->toDecisionTreeFactor())); DiscreteFactor::shared_ptr reduced4 = singleValue.partiallyApply(domains); - EXPECT(assert_equal(f4,reduced4->toDecisionTreeFactor())); + EXPECT(assert_equal(f4, reduced4->toDecisionTreeFactor())); // full arc-consistency test csp.runArcConsistency(nrColors); + // GTSAM_PRINT(csp); } /* ************************************************************************* */ @@ -229,4 +276,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index 9929938d5..c48d7639d 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -5,14 +5,15 @@ * @date Oct 11, 2013 */ -#include +#include #include #include -#include -#include +#include + #include -#include +#include #include +#include using namespace std; using namespace boost; @@ -23,11 +24,12 @@ using namespace gtsam; * Loopy belief solver for graphs with only binary and unary factors */ class LoopyBelief { - /** Star graph struct for each node, containing * - the star graph itself - * - the product of original unary factors so we don't have to recompute it later, and - * - the factor indices of the corrected belief factors of the neighboring nodes + * - the product of original unary factors so we don't have to recompute it + * later, and + * - the factor indices of the corrected belief factors of the neighboring + * nodes */ typedef std::map CorrectedBeliefIndices; struct StarGraph { @@ -36,41 +38,41 @@ class LoopyBelief { DecisionTreeFactor::shared_ptr unary; VariableIndex varIndex_; StarGraph(const DiscreteFactorGraph::shared_ptr& _star, - const CorrectedBeliefIndices& _beliefIndices, - const DecisionTreeFactor::shared_ptr& _unary) : - star(_star), correctedBeliefIndices(_beliefIndices), unary(_unary), varIndex_( - *_star) { - } + const CorrectedBeliefIndices& _beliefIndices, + const DecisionTreeFactor::shared_ptr& _unary) + : star(_star), + correctedBeliefIndices(_beliefIndices), + unary(_unary), + varIndex_(*_star) {} void print(const std::string& s = "") const { cout << s << ":" << endl; star->print("Star graph: "); - for(Key key: correctedBeliefIndices | boost::adaptors::map_keys) { + for (Key key : correctedBeliefIndices | boost::adaptors::map_keys) { cout << "Belief factor index for " << key << ": " - << correctedBeliefIndices.at(key) << endl; + << correctedBeliefIndices.at(key) << endl; } - if (unary) - unary->print("Unary: "); + if (unary) unary->print("Unary: "); } }; typedef std::map StarGraphs; - StarGraphs starGraphs_; ///< star graph at each variable + StarGraphs starGraphs_; ///< star graph at each variable -public: + public: /** Constructor - * Need all discrete keys to access node's cardinality for creating belief factors + * Need all discrete keys to access node's cardinality for creating belief + * factors * TODO: so troublesome!! */ LoopyBelief(const DiscreteFactorGraph& graph, - const std::map& allDiscreteKeys) : - starGraphs_(buildStarGraphs(graph, allDiscreteKeys)) { - } + const std::map& allDiscreteKeys) + : starGraphs_(buildStarGraphs(graph, allDiscreteKeys)) {} /// print void print(const std::string& s = "") const { cout << s << ":" << endl; - for(Key key: starGraphs_ | boost::adaptors::map_keys) { + for (Key key : starGraphs_ | boost::adaptors::map_keys) { starGraphs_.at(key).print((boost::format("Node %d:") % key).str()); } } @@ -79,12 +81,13 @@ public: DiscreteFactorGraph::shared_ptr iterate( const std::map& allDiscreteKeys) { static const bool debug = false; - static DiscreteConditional::shared_ptr dummyCond; // unused by-product of elimination + static DiscreteConditional::shared_ptr + dummyCond; // unused by-product of elimination DiscreteFactorGraph::shared_ptr beliefs(new DiscreteFactorGraph()); std::map > allMessages; // Eliminate each star graph - for(Key key: starGraphs_ | boost::adaptors::map_keys) { -// cout << "***** Node " << key << "*****" << endl; + for (Key key : starGraphs_ | boost::adaptors::map_keys) { + // cout << "***** Node " << key << "*****" << endl; // initialize belief to the unary factor from the original graph DecisionTreeFactor::shared_ptr beliefAtKey; @@ -92,15 +95,16 @@ public: std::map messages; // eliminate each neighbor in this star graph one by one - for(Key neighbor: starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) { + for (Key neighbor : starGraphs_.at(key).correctedBeliefIndices | + boost::adaptors::map_keys) { DiscreteFactorGraph subGraph; - for(size_t factor: starGraphs_.at(key).varIndex_[neighbor]) { + for (size_t factor : starGraphs_.at(key).varIndex_[neighbor]) { subGraph.push_back(starGraphs_.at(key).star->at(factor)); } if (debug) subGraph.print("------- Subgraph:"); DiscreteFactor::shared_ptr message; - boost::tie(dummyCond, message) = EliminateDiscrete(subGraph, - Ordering(list_of(neighbor))); + boost::tie(dummyCond, message) = + EliminateDiscrete(subGraph, Ordering(list_of(neighbor))); // store the new factor into messages messages.insert(make_pair(neighbor, message)); if (debug) message->print("------- Message: "); @@ -108,14 +112,12 @@ public: // Belief is the product of all messages and the unary factor // Incorporate new the factor to belief if (!beliefAtKey) - beliefAtKey = boost::dynamic_pointer_cast( - message); - else beliefAtKey = - boost::make_shared( - (*beliefAtKey) - * (*boost::dynamic_pointer_cast( - message))); + boost::dynamic_pointer_cast(message); + else + beliefAtKey = boost::make_shared( + (*beliefAtKey) * + (*boost::dynamic_pointer_cast(message))); } if (starGraphs_.at(key).unary) beliefAtKey = boost::make_shared( @@ -133,7 +135,8 @@ public: sumFactorTable = (boost::format("%s %f") % sumFactorTable % sum).str(); DecisionTreeFactor sumFactor(allDiscreteKeys.at(key), sumFactorTable); if (debug) sumFactor.print("denomFactor: "); - beliefAtKey = boost::make_shared((*beliefAtKey) / sumFactor); + beliefAtKey = + boost::make_shared((*beliefAtKey) / sumFactor); if (debug) beliefAtKey->print("New belief at key normalized: "); beliefs->push_back(beliefAtKey); allMessages[key] = messages; @@ -141,17 +144,20 @@ public: // Update corrected beliefs VariableIndex beliefFactors(*beliefs); - for(Key key: starGraphs_ | boost::adaptors::map_keys) { + for (Key key : starGraphs_ | boost::adaptors::map_keys) { std::map messages = allMessages[key]; - for(Key neighbor: starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) { - DecisionTreeFactor correctedBelief = (*boost::dynamic_pointer_cast< - DecisionTreeFactor>(beliefs->at(beliefFactors[key].front()))) - / (*boost::dynamic_pointer_cast( + for (Key neighbor : starGraphs_.at(key).correctedBeliefIndices | + boost::adaptors::map_keys) { + DecisionTreeFactor correctedBelief = + (*boost::dynamic_pointer_cast( + beliefs->at(beliefFactors[key].front()))) / + (*boost::dynamic_pointer_cast( messages.at(neighbor))); if (debug) correctedBelief.print("correctedBelief: "); - size_t beliefIndex = starGraphs_.at(neighbor).correctedBeliefIndices.at( - key); - starGraphs_.at(neighbor).star->replace(beliefIndex, + size_t beliefIndex = + starGraphs_.at(neighbor).correctedBeliefIndices.at(key); + starGraphs_.at(neighbor).star->replace( + beliefIndex, boost::make_shared(correctedBelief)); } } @@ -161,21 +167,22 @@ public: return beliefs; } -private: + private: /** * Build star graphs for each node. */ - StarGraphs buildStarGraphs(const DiscreteFactorGraph& graph, + StarGraphs buildStarGraphs( + const DiscreteFactorGraph& graph, const std::map& allDiscreteKeys) const { StarGraphs starGraphs; - VariableIndex varIndex(graph); ///< access to all factors of each node - for(Key key: varIndex | boost::adaptors::map_keys) { + VariableIndex varIndex(graph); ///< access to all factors of each node + for (Key key : varIndex | boost::adaptors::map_keys) { // initialize to multiply with other unary factors later DecisionTreeFactor::shared_ptr prodOfUnaries; // collect all factors involving this key in the original graph DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph()); - for(size_t factorIndex: varIndex[key]) { + for (size_t factorIndex : varIndex[key]) { star->push_back(graph.at(factorIndex)); // accumulate unary factors @@ -185,9 +192,9 @@ private: graph.at(factorIndex)); else prodOfUnaries = boost::make_shared( - *prodOfUnaries - * (*boost::dynamic_pointer_cast( - graph.at(factorIndex)))); + *prodOfUnaries * + (*boost::dynamic_pointer_cast( + graph.at(factorIndex)))); } } @@ -196,7 +203,7 @@ private: KeySet neighbors = star->keys(); neighbors.erase(key); CorrectedBeliefIndices correctedBeliefIndices; - for(Key neighbor: neighbors) { + for (Key neighbor : neighbors) { // TODO: default table for keys with more than 2 values? string initialBelief; for (size_t v = 0; v < allDiscreteKeys.at(neighbor).second - 1; ++v) { @@ -207,9 +214,8 @@ private: DecisionTreeFactor(allDiscreteKeys.at(neighbor), initialBelief)); correctedBeliefIndices.insert(make_pair(neighbor, star->size() - 1)); } - starGraphs.insert( - make_pair(key, - StarGraph(star, correctedBeliefIndices, prodOfUnaries))); + starGraphs.insert(make_pair( + key, StarGraph(star, correctedBeliefIndices, prodOfUnaries))); } return starGraphs; } @@ -249,7 +255,6 @@ TEST_UNSAFE(LoopyBelief, construction) { DiscreteFactorGraph::shared_ptr beliefs = solver.iterate(allKeys); beliefs->print(); } - } /* ************************************************************************* */ diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp index 3f6c6a1e0..7822cbd38 100644 --- a/gtsam_unstable/discrete/tests/testScheduler.cpp +++ b/gtsam_unstable/discrete/tests/testScheduler.cpp @@ -5,14 +5,13 @@ */ //#define ENABLE_TIMING -#include +#include #include #include +#include -#include - -#include #include +#include #include using namespace boost::assign; @@ -22,7 +21,6 @@ using namespace gtsam; /* ************************************************************************* */ // Create the expected graph of constraints DiscreteFactorGraph createExpected() { - // Start building size_t nrFaculty = 4, nrTimeSlots = 3; @@ -47,27 +45,27 @@ DiscreteFactorGraph createExpected() { string available = "1 1 1 0 1 1 1 1 0 1 1 1"; // Akansel - expected.add(A1, faculty_in_A); // Area 1 - expected.add(A1, "1 1 1 0"); // Advisor + expected.add(A1, faculty_in_A); // Area 1 + expected.add(A1, "1 1 1 0"); // Advisor expected.add(A & A1, available); - expected.add(A2, faculty_in_M); // Area 2 - expected.add(A2, "1 1 1 0"); // Advisor + expected.add(A2, faculty_in_M); // Area 2 + expected.add(A2, "1 1 1 0"); // Advisor expected.add(A & A2, available); - expected.add(A3, faculty_in_P); // Area 3 - expected.add(A3, "1 1 1 0"); // Advisor + expected.add(A3, faculty_in_P); // Area 3 + expected.add(A3, "1 1 1 0"); // Advisor expected.add(A & A3, available); // Mutual exclusion for faculty expected.addAllDiff(A1 & A2 & A3); // Jake - expected.add(J1, faculty_in_H); // Area 1 - expected.add(J1, "1 0 1 1"); // Advisor + expected.add(J1, faculty_in_H); // Area 1 + expected.add(J1, "1 0 1 1"); // Advisor expected.add(J & J1, available); - expected.add(J2, faculty_in_C); // Area 2 - expected.add(J2, "1 0 1 1"); // Advisor + expected.add(J2, faculty_in_C); // Area 2 + expected.add(J2, "1 0 1 1"); // Advisor expected.add(J & J2, available); - expected.add(J3, faculty_in_A); // Area 3 - expected.add(J3, "1 0 1 1"); // Advisor + expected.add(J3, faculty_in_A); // Area 3 + expected.add(J3, "1 0 1 1"); // Advisor expected.add(J & J3, available); // Mutual exclusion for faculty expected.addAllDiff(J1 & J2 & J3); @@ -79,8 +77,7 @@ DiscreteFactorGraph createExpected() { } /* ************************************************************************* */ -TEST( schedulingExample, test) -{ +TEST(schedulingExample, test) { Scheduler s(2); // add faculty @@ -121,33 +118,32 @@ TEST( schedulingExample, test) // Do brute force product and output that to file DecisionTreeFactor product = s.product(); - //product.dot("scheduling", false); + // product.dot("scheduling", false); // Do exact inference gttic(small); - DiscreteFactor::sharedValues MPE = s.optimalAssignment(); + auto MPE = s.optimalAssignment(); gttoc(small); // print MPE, commented out as unit tests don't print -// s.printAssignment(MPE); + // s.printAssignment(MPE); // Commented out as does not work yet // s.runArcConsistency(8,10,true); // find the assignment of students to slots with most possible committees // Commented out as not implemented yet -// sharedValues bestSchedule = s.bestSchedule(); -// GTSAM_PRINT(*bestSchedule); + // auto bestSchedule = s.bestSchedule(); + // GTSAM_PRINT(bestSchedule); // find the corresponding most desirable committee assignment // Commented out as not implemented yet -// sharedValues bestAssignment = s.bestAssignment(bestSchedule); -// GTSAM_PRINT(*bestAssignment); + // auto bestAssignment = s.bestAssignment(bestSchedule); + // GTSAM_PRINT(bestAssignment); } /* ************************************************************************* */ -TEST( schedulingExample, smallFromFile) -{ +TEST(schedulingExample, smallFromFile) { string path(TOPSRCDIR "/gtsam_unstable/discrete/examples/"); Scheduler s(2, path + "small.csv"); @@ -179,4 +175,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam_unstable/discrete/tests/testSudoku.cpp b/gtsam_unstable/discrete/tests/testSudoku.cpp index e2115e8bc..808f98b1c 100644 --- a/gtsam_unstable/discrete/tests/testSudoku.cpp +++ b/gtsam_unstable/discrete/tests/testSudoku.cpp @@ -5,74 +5,69 @@ * @author Frank Dellaert */ -#include #include +#include +#include + #include using boost::assign::insert; +#include + #include #include -#include using namespace std; using namespace gtsam; #define PRINT false -class Sudoku: public CSP { +/// A class that encodes Sudoku's as a CSP problem +class Sudoku : public CSP { + size_t n_; ///< Side of Sudoku, e.g. 4 or 9 - /// sudoku size - size_t n_; - - /// discrete keys - typedef std::pair IJ; + /// Mapping from base i,j coordinates to discrete keys: + using IJ = std::pair; std::map dkeys_; -public: - + public: /// return DiscreteKey for cell(i,j) const DiscreteKey& dkey(size_t i, size_t j) const { return dkeys_.at(IJ(i, j)); } /// return Key for cell(i,j) - Key key(size_t i, size_t j) const { - return dkey(i, j).first; - } + Key key(size_t i, size_t j) const { return dkey(i, j).first; } /// Constructor - Sudoku(size_t n, ...) : - n_(n) { + Sudoku(size_t n, ...) : n_(n) { // Create variables, ordering, and unary constraints va_list ap; va_start(ap, n); - Key k=0; for (size_t i = 0; i < n; ++i) { - for (size_t j = 0; j < n; ++j, ++k) { + for (size_t j = 0; j < n; ++j) { // create the key IJ ij(i, j); - dkeys_[ij] = DiscreteKey(k, n); + Symbol key('1' + i, j + 1); + dkeys_[ij] = DiscreteKey(key, n); // get the unary constraint, if any int value = va_arg(ap, int); - // cout << value << " "; if (value != 0) addSingleValue(dkeys_[ij], value - 1); } - //cout << endl; + // cout << endl; } va_end(ap); // add row constraints for (size_t i = 0; i < n; i++) { DiscreteKeys dkeys; - for (size_t j = 0; j < n; j++) - dkeys += dkey(i, j); + for (size_t j = 0; j < n; j++) dkeys += dkey(i, j); addAllDiff(dkeys); } // add col constraints for (size_t j = 0; j < n; j++) { DiscreteKeys dkeys; - for (size_t i = 0; i < n; i++) - dkeys += dkey(i, j); + for (size_t i = 0; i < n; i++) dkeys += dkey(i, j); addAllDiff(dkeys); } @@ -84,8 +79,7 @@ public: // Box I,J DiscreteKeys dkeys; for (size_t i = i0; i < i0 + N; i++) - for (size_t j = j0; j < j0 + N; j++) - dkeys += dkey(i, j); + for (size_t j = j0; j < j0 + N; j++) dkeys += dkey(i, j); addAllDiff(dkeys); j0 += N; } @@ -94,120 +88,171 @@ public: } /// Print readable form of assignment - void printAssignment(DiscreteFactor::sharedValues assignment) const { + void printAssignment(const DiscreteFactor::Values& assignment) const { for (size_t i = 0; i < n_; i++) { for (size_t j = 0; j < n_; j++) { Key k = key(i, j); - cout << 1 + assignment->at(k) << " "; + cout << 1 + assignment.at(k) << " "; } cout << endl; } } /// solve and print solution - void printSolution() { - DiscreteFactor::sharedValues MPE = optimalAssignment(); + void printSolution() const { + auto MPE = optimalAssignment(); printAssignment(MPE); } + // Print domain + void printDomains(const Domains& domains) { + for (size_t i = 0; i < n_; i++) { + for (size_t j = 0; j < n_; j++) { + Key k = key(i, j); + cout << domains.at(k).base1Str(); + cout << "\t"; + } // i + cout << endl; + } // j + } }; /* ************************************************************************* */ -TEST_UNSAFE( Sudoku, small) -{ - Sudoku csp(4, - 1,0, 0,4, - 0,0, 0,0, - - 4,0, 2,0, - 0,1, 0,0); - - // Do BP - csp.runArcConsistency(4,10,PRINT); +TEST(Sudoku, small) { + Sudoku csp(4, // + 1, 0, 0, 4, // + 0, 0, 0, 0, // + 4, 0, 2, 0, // + 0, 1, 0, 0); // optimize and check - CSP::sharedValues solution = csp.optimalAssignment(); + auto solution = csp.optimalAssignment(); CSP::Values expected; - insert(expected) - (csp.key(0,0), 0)(csp.key(0,1), 1)(csp.key(0,2), 2)(csp.key(0,3), 3) - (csp.key(1,0), 2)(csp.key(1,1), 3)(csp.key(1,2), 0)(csp.key(1,3), 1) - (csp.key(2,0), 3)(csp.key(2,1), 2)(csp.key(2,2), 1)(csp.key(2,3), 0) - (csp.key(3,0), 1)(csp.key(3,1), 0)(csp.key(3,2), 3)(csp.key(3,3), 2); - EXPECT(assert_equal(expected,*solution)); - //csp.printAssignment(solution); + insert(expected)(csp.key(0, 0), 0)(csp.key(0, 1), 1)(csp.key(0, 2), 2)( + csp.key(0, 3), 3)(csp.key(1, 0), 2)(csp.key(1, 1), 3)(csp.key(1, 2), 0)( + csp.key(1, 3), 1)(csp.key(2, 0), 3)(csp.key(2, 1), 2)(csp.key(2, 2), 1)( + csp.key(2, 3), 0)(csp.key(3, 0), 1)(csp.key(3, 1), 0)(csp.key(3, 2), 3)( + csp.key(3, 3), 2); + EXPECT(assert_equal(expected, solution)); + // csp.printAssignment(solution); + + // Do BP (AC1) + auto domains = csp.runArcConsistency(4, 3); + // csp.printDomains(domains); + Domain domain44 = domains.at(Symbol('4', 4)); + EXPECT_LONGS_EQUAL(1, domain44.nrValues()); + + // Test Creation of a new, simpler CSP + CSP new_csp = csp.partiallyApply(domains); + // Should only be 16 new Domains + EXPECT_LONGS_EQUAL(16, new_csp.size()); + + // Check that solution + auto new_solution = new_csp.optimalAssignment(); + // csp.printAssignment(new_solution); + EXPECT(assert_equal(expected, new_solution)); } /* ************************************************************************* */ -TEST_UNSAFE( Sudoku, easy) -{ - Sudoku sudoku(9, - 0,0,5, 0,9,0, 0,0,1, - 0,0,0, 0,0,2, 0,7,3, - 7,6,0, 0,0,8, 2,0,0, +TEST(Sudoku, easy) { + Sudoku csp(9, // + 0, 0, 5, 0, 9, 0, 0, 0, 1, // + 0, 0, 0, 0, 0, 2, 0, 7, 3, // + 7, 6, 0, 0, 0, 8, 2, 0, 0, // - 0,1,2, 0,0,9, 0,0,4, - 0,0,0, 2,0,3, 0,0,0, - 3,0,0, 1,0,0, 9,6,0, + 0, 1, 2, 0, 0, 9, 0, 0, 4, // + 0, 0, 0, 2, 0, 3, 0, 0, 0, // + 3, 0, 0, 1, 0, 0, 9, 6, 0, // - 0,0,1, 9,0,0, 0,5,8, - 9,7,0, 5,0,0, 0,0,0, - 5,0,0, 0,3,0, 7,0,0); + 0, 0, 1, 9, 0, 0, 0, 5, 8, // + 9, 7, 0, 5, 0, 0, 0, 0, 0, // + 5, 0, 0, 0, 3, 0, 7, 0, 0); - // Do BP - sudoku.runArcConsistency(4,10,PRINT); + // csp.printSolution(); // don't do it - // sudoku.printSolution(); // don't do it + // Do BP (AC1) + auto domains = csp.runArcConsistency(9, 10); + // csp.printDomains(domains); + Key key99 = Symbol('9', 9); + Domain domain99 = domains.at(key99); + EXPECT_LONGS_EQUAL(1, domain99.nrValues()); + + // Test Creation of a new, simpler CSP + CSP new_csp = csp.partiallyApply(domains); + // 81 new Domains, and still 26 all-diff constraints + EXPECT_LONGS_EQUAL(81 + 26, new_csp.size()); + + // csp.printSolution(); // still don't do it ! :-( } /* ************************************************************************* */ -TEST_UNSAFE( Sudoku, extreme) -{ - Sudoku sudoku(9, - 0,0,9, 7,4,8, 0,0,0, - 7,0,0, 0,0,0, 0,0,0, - 0,2,0, 1,0,9, 0,0,0, - - 0,0,7, 0,0,0, 2,4,0, - 0,6,4, 0,1,0, 5,9,0, - 0,9,8, 0,0,0, 3,0,0, - - 0,0,0, 8,0,3, 0,2,0, - 0,0,0, 0,0,0, 0,0,6, - 0,0,0, 2,7,5, 9,0,0); +TEST(Sudoku, extreme) { + Sudoku csp(9, // + 0, 0, 9, 7, 4, 8, 0, 0, 0, 7, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, // + 0, 1, 0, 9, 0, 0, 0, 0, 0, 7, // + 0, 0, 0, 2, 4, 0, 0, 6, 4, 0, // + 1, 0, 5, 9, 0, 0, 9, 8, 0, 0, // + 0, 3, 0, 0, 0, 0, 0, 8, 0, 3, // + 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 6, 0, 0, 0, 2, 7, 5, 9, 0, 0); // Do BP - sudoku.runArcConsistency(9,10,PRINT); + csp.runArcConsistency(9, 10); #ifdef METIS - VariableIndexOrdered index(sudoku); + VariableIndexOrdered index(csp); index.print("index"); ofstream os("/Users/dellaert/src/hmetis-1.5-osx-i686/extreme-dual.txt"); index.outputMetisFormat(os); #endif - //sudoku.printSolution(); // don't do it + // Do BP (AC1) + auto domains = csp.runArcConsistency(9, 10); + // csp.printDomains(domains); + Key key99 = Symbol('9', 9); + Domain domain99 = domains.at(key99); + EXPECT_LONGS_EQUAL(2, domain99.nrValues()); + + // Test Creation of a new, simpler CSP + CSP new_csp = csp.partiallyApply(domains); + // 81 new Domains, and still 20 all-diff constraints + EXPECT_LONGS_EQUAL(81 + 20, new_csp.size()); + + // csp.printSolution(); // still don't do it ! :-( } /* ************************************************************************* */ -TEST_UNSAFE( Sudoku, AJC_3star_Feb8_2012) -{ - Sudoku sudoku(9, - 9,5,0, 0,0,6, 0,0,0, - 0,8,4, 0,7,0, 0,0,0, - 6,2,0, 5,0,0, 4,0,0, +TEST(Sudoku, AJC_3star_Feb8_2012) { + Sudoku csp(9, // + 9, 5, 0, 0, 0, 6, 0, 0, 0, // + 0, 8, 4, 0, 7, 0, 0, 0, 0, // + 6, 2, 0, 5, 0, 0, 4, 0, 0, // - 0,0,0, 2,9,0, 6,0,0, - 0,9,0, 0,0,0, 0,2,0, - 0,0,2, 0,6,3, 0,0,0, + 0, 0, 0, 2, 9, 0, 6, 0, 0, // + 0, 9, 0, 0, 0, 0, 0, 2, 0, // + 0, 0, 2, 0, 6, 3, 0, 0, 0, // - 0,0,9, 0,0,7, 0,6,8, - 0,0,0, 0,3,0, 2,9,0, - 0,0,0, 1,0,0, 0,3,7); + 0, 0, 9, 0, 0, 7, 0, 6, 8, // + 0, 0, 0, 0, 3, 0, 2, 9, 0, // + 0, 0, 0, 1, 0, 0, 0, 3, 7); - // Do BP - sudoku.runArcConsistency(9,10,PRINT); + // Do BP (AC1) + auto domains = csp.runArcConsistency(9, 10); + // csp.printDomains(domains); + Key key99 = Symbol('9', 9); + Domain domain99 = domains.at(key99); + EXPECT_LONGS_EQUAL(1, domain99.nrValues()); - //sudoku.printSolution(); // don't do it + // Test Creation of a new, simpler CSP + CSP new_csp = csp.partiallyApply(domains); + // Just the 81 new Domains + EXPECT_LONGS_EQUAL(81, new_csp.size()); + + // Check that solution + auto solution = new_csp.optimalAssignment(); + // csp.printAssignment(solution); + EXPECT_LONGS_EQUAL(6, solution.at(key99)); } /* ************************************************************************* */ @@ -216,4 +261,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 67a0c971e..12bd93416 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -226,7 +226,7 @@ pair testParser(QPSParser parser) { expected.inequalities.add(X1, -I_1x1, 0, 2); // x >= 0 expected.inequalities.add(X2, -I_1x1, 0, 3); // y > = 0 return {expected, exampleqp}; -}; +} TEST(QPSolver, ParserSyntaticTest) { auto result = testParser(QPSParser("QPExample.QPS")); diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 98ec59fe9..572935da3 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -56,7 +56,8 @@ private: bool flag_bump_up_near_zero_probs_; /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T)GTSAM_CONCEPT_TESTABLE_TYPE(T) + GTSAM_CONCEPT_LIE_TYPE(T) + GTSAM_CONCEPT_TESTABLE_TYPE(T) public: diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index b703f5900..e2444a51a 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -89,12 +89,19 @@ set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name ) +# Set the path for the GTSAM python module set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam) -# Symlink all tests .py files to build folder. +# Copy all python files to build folder. copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" "${GTSAM_MODULE_PATH}") +# Hack to get python test files copied every time they are modified +file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py") +foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) + configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY) +endforeach() + # Common directory for data/datasets stored with the package. # This will store the data in the Python site package directly. file(COPY "${GTSAM_SOURCE_DIR}/examples/Data" DESTINATION "${GTSAM_MODULE_PATH}") @@ -147,10 +154,16 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) set(GTSAM_UNSTABLE_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable) - # Symlink all tests .py files to build folder. + # Copy all python files to build folder. copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable" "${GTSAM_UNSTABLE_MODULE_PATH}") + # Hack to get python test files copied every time they are modified + file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py") + foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) + configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY) + endforeach() + # Add gtsam_unstable to the install target list(APPEND GTSAM_PYTHON_DEPENDENCIES ${GTSAM_PYTHON_UNSTABLE_TARGET}) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index dfe8b523c..f3e48c3c3 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -7,73 +7,79 @@ See LICENSE for the license information Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file - Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert) + Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert, Varun Agrawal) """ import argparse import logging import sys -import matplotlib.pyplot as plt -import numpy as np - import gtsam -from gtsam import ( - GeneralSFMFactorCal3Bundler, - PinholeCameraCal3Bundler, - PriorFactorPinholeCameraCal3Bundler, - readBal, - symbol_shorthand -) +from gtsam import (GeneralSFMFactorCal3Bundler, + PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3) +from gtsam.symbol_shorthand import C, P # type: ignore +from gtsam.utils import plot # type: ignore +from matplotlib import pyplot as plt -C = symbol_shorthand.C -P = symbol_shorthand.P +logging.basicConfig(stream=sys.stdout, level=logging.INFO) + +DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre" -logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) +def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None: + """Plot the SFM results.""" + plot_vals = gtsam.Values() + for cam_idx in range(scene_data.number_cameras()): + plot_vals.insert(C(cam_idx), + result.atPinholeCameraCal3Bundler(C(cam_idx)).pose()) + for j in range(scene_data.number_tracks()): + plot_vals.insert(P(j), result.atPoint3(P(j))) -def run(args): + plot.plot_3d_points(0, plot_vals, linespec="g.") + plot.plot_trajectory(0, plot_vals, title="SFM results") + + plt.show() + + +def run(args: argparse.Namespace) -> None: """ Run LM optimization with BAL input data and report resulting error """ - input_file = gtsam.findExampleDataFile(args.input_file) + input_file = args.input_file # Load the SfM data from file - scene_data = readBal(input_file) - logging.info(f"read {scene_data.number_tracks()} tracks on {scene_data.number_cameras()} cameras\n") + scene_data = gtsam.readBal(input_file) + logging.info("read %d tracks on %d cameras\n", scene_data.number_tracks(), + scene_data.number_cameras()) # Create a factor graph graph = gtsam.NonlinearFactorGraph() # We share *one* noiseModel between all projection factors - noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v + noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Add measurements to the factor graph - j = 0 - for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) # SfmTrack + for j in range(scene_data.number_tracks()): + track = scene_data.track(j) # SfmTrack # retrieve the SfmMeasurement objects for m_idx in range(track.number_measurements()): # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(m_idx) # note use of shorthand symbols C and P graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) - j += 1 # Add a prior on pose x1. This indirectly specifies where the origin is. graph.push_back( - gtsam.PriorFactorPinholeCameraCal3Bundler( - C(0), scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) - ) - ) + PriorFactorPinholeCameraCal3Bundler( + C(0), scene_data.camera(0), + gtsam.noiseModel.Isotropic.Sigma(9, 0.1))) # Also add a prior on the position of the first landmark to fix the scale graph.push_back( - gtsam.PriorFactorPoint3( - P(0), scene_data.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - ) - ) + PriorFactorPoint3(P(0), + scene_data.track(0).point3(), + gtsam.noiseModel.Isotropic.Sigma(3, 0.1))) # Create initial estimate initial = gtsam.Values() - + i = 0 # add each PinholeCameraCal3Bundler for cam_idx in range(scene_data.number_cameras()): @@ -81,12 +87,10 @@ def run(args): initial.insert(C(i), camera) i += 1 - j = 0 # add each SfmTrack - for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) + for j in range(scene_data.number_tracks()): + track = scene_data.track(j) initial.insert(P(j), track.point3()) - j += 1 # Optimize the graph and print results try: @@ -94,25 +98,31 @@ def run(args): params.setVerbosityLM("ERROR") lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = lm.optimize() - except Exception as e: + except RuntimeError: logging.exception("LM Optimization failed") return + # Error drops from ~2764.22 to ~0.046 - logging.info(f"final error: {graph.error(result)}") + logging.info("initial error: %f", graph.error(initial)) + logging.info("final error: %f", graph.error(result)) + + plot_scene(scene_data, result) + + +def main() -> None: + """Main runner.""" + parser = argparse.ArgumentParser() + parser.add_argument('-i', + '--input_file', + type=str, + default=gtsam.findExampleDataFile(DEFAULT_BAL_DATASET), + help="""Read SFM data from the specified BAL file. + The data format is described here: https://grail.cs.washington.edu/projects/bal/. + BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, + then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector + and (x,y,z) 3d point initializations.""") + run(parser.parse_args()) if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument( - '-i', - '--input_file', - type=str, - default="dubrovnik-3-7-pre", - help='Read SFM data from the specified BAL file' - 'The data format is described here: https://grail.cs.washington.edu/projects/bal/.' - 'BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, ' - 'then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector' - 'and (x,y,z) 3d point initializations.' - ) - run(parser.parse_args()) - + main() diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 298c6e57b..e54afc757 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -17,6 +17,15 @@ import gtsam from gtsam.utils.test_case import GtsamTestCase from gtsam.symbol_shorthand import K, L, P + +def ulp(ftype=np.float64): + """ + Unit in the last place of floating point datatypes + """ + f = np.finfo(ftype) + return f.tiny / ftype(1 << f.nmant) + + class TestCal3Fisheye(GtsamTestCase): @classmethod @@ -105,6 +114,71 @@ class TestCal3Fisheye(GtsamTestCase): score = graph.error(state) self.assertAlmostEqual(score, 0) + def test_jacobian_on_axis(self): + """Check of jacobian at optical axis""" + obj_point_on_axis = np.array([0, 0, 1]) + img_point = np.array([0, 0]) + f, z, H = self.evaluate_jacobian(obj_point_on_axis, img_point) + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 3*np.eye(2)) + + def test_jacobian_convergence(self): + """Test stability of jacobian close to optical axis""" + t = ulp(np.float64) + obj_point_close_to_axis = np.array([t, 0, 1]) + img_point = np.array([np.sqrt(t), 0]) + f, z, H = self.evaluate_jacobian(obj_point_close_to_axis, img_point) + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 3*np.eye(2)) + + # With a height of sqrt(ulp), this may cause an overflow + t = ulp(np.float64) + obj_point_close_to_axis = np.array([np.sqrt(t), 0, 1]) + img_point = np.array([np.sqrt(t), 0]) + f, z, H = self.evaluate_jacobian(obj_point_close_to_axis, img_point) + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 3*np.eye(2)) + + def test_scaling_factor(self): + """Check convergence of atan2(r, z)/r ~ 1/z for small r""" + r = ulp(np.float64) + s = np.arctan(r) / r + self.assertEqual(s, 1.0) + z = 1 + s = self.scaling_factor(r, z) + self.assertEqual(s, 1.0/z) + z = 2 + s = self.scaling_factor(r, z) + self.assertEqual(s, 1.0/z) + s = self.scaling_factor(2*r, z) + self.assertEqual(s, 1.0/z) + + @staticmethod + def scaling_factor(r, z): + """Projection factor theta/r for equidistant fisheye lens model""" + return np.arctan2(r, z) / r if r/z != 0 else 1.0/z + + @staticmethod + def evaluate_jacobian(obj_point, img_point): + """Evaluate jacobian at given object point""" + pose = gtsam.Pose3() + camera = gtsam.Cal3Fisheye() + state = gtsam.Values() + camera_key, pose_key, landmark_key = K(0), P(0), L(0) + state.insert_point3(landmark_key, obj_point) + state.insert_pose3(pose_key, pose) + g = gtsam.NonlinearFactorGraph() + noise_model = gtsam.noiseModel.Unit.Create(2) + factor = gtsam.GenericProjectionFactorCal3Fisheye(img_point, noise_model, pose_key, landmark_key, camera) + g.add(factor) + f = g.error(state) + gaussian_factor_graph = g.linearize(state) + H, z = gaussian_factor_graph.jacobian() + return f, z, H + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation_skipped(self): """Estimate spatial point from image measurements""" diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index dab1ae446..bafbacfa4 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -117,6 +117,28 @@ class TestCal3Unified(GtsamTestCase): score = graph.error(state) self.assertAlmostEqual(score, 0) + def test_jacobian(self): + """Evaluate jacobian at optical axis""" + obj_point_on_axis = np.array([0, 0, 1]) + img_point = np.array([0.0, 0.0]) + pose = gtsam.Pose3() + camera = gtsam.Cal3Unified() + state = gtsam.Values() + camera_key, pose_key, landmark_key = K(0), P(0), L(0) + state.insert_cal3unified(camera_key, camera) + state.insert_point3(landmark_key, obj_point_on_axis) + state.insert_pose3(pose_key, pose) + g = gtsam.NonlinearFactorGraph() + noise_model = gtsam.noiseModel.Unit.Create(2) + factor = gtsam.GeneralSFMFactor2Cal3Unified(img_point, noise_model, pose_key, landmark_key, camera_key) + g.add(factor) + f = g.error(state) + gaussian_factor_graph = g.linearize(state) + H, z = gaussian_factor_graph.jacobian() + self.assertAlmostEqual(f, 0) + self.gtsamAssertEquals(z, np.zeros(2)) + self.gtsamAssertEquals(H @ H.T, 4*np.eye(2)) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation(self): """Estimate spatial point from image measurements""" diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index e07b904a9..411828890 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -59,8 +59,16 @@ class TestPose3(GtsamTestCase): self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2)) def test_adjoint(self): - """Test adjoint method.""" + """Test adjoint methods.""" + T = Pose3() xi = np.array([1, 2, 3, 4, 5, 6]) + # test calling functions + T.AdjointMap() + T.Adjoint(xi) + T.AdjointTranspose(xi) + Pose3.adjointMap(xi) + Pose3.adjoint(xi, xi) + # test correctness of adjoint(x, y) expected = np.dot(Pose3.adjointMap_(xi), xi) actual = Pose3.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 66dbed1eb..75425a0cd 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -606,7 +606,7 @@ Vector3 f(const Point2& a, const Vector3& b, OptionalJacobian<3, 2> H1, if (H1) *H1 << b.y(), b.z(), b.x(), 0, 0, 0; if (H2) *H2 = A; return A * b; -}; +} } TEST(ExpressionFactor, MultiplyWithInverseFunction) { diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 84bba850b..67a23355d 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -101,6 +101,82 @@ TEST( NonlinearFactor, NonlinearFactor ) DOUBLES_EQUAL(expected,actual,0.00000001); } +/* ************************************************************************* */ +TEST(NonlinearFactor, Weight) { + // create a values structure for the non linear factor graph + Values values; + + // Instantiate a concrete class version of a NoiseModelFactor + PriorFactor factor1(X(1), Point2(0, 0)); + values.insert(X(1), Point2(0.1, 0.1)); + + CHECK(assert_equal(1.0, factor1.weight(values))); + + // Factor with noise model + auto noise = noiseModel::Isotropic::Sigma(2, 0.2); + PriorFactor factor2(X(2), Point2(1, 1), noise); + values.insert(X(2), Point2(1.1, 1.1)); + + CHECK(assert_equal(1.0, factor2.weight(values))); + + Point2 estimate(3, 3), prior(1, 1); + double distance = (estimate - prior).norm(); + + auto gaussian = noiseModel::Isotropic::Sigma(2, 0.2); + + PriorFactor factor; + + // vector to store all the robust models in so we can test iteratively. + vector robust_models; + + // Fair noise model + auto fair = noiseModel::Robust::Create( + noiseModel::mEstimator::Fair::Create(1.3998), gaussian); + robust_models.push_back(fair); + + // Huber noise model + auto huber = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), gaussian); + robust_models.push_back(huber); + + // Cauchy noise model + auto cauchy = noiseModel::Robust::Create( + noiseModel::mEstimator::Cauchy::Create(0.1), gaussian); + robust_models.push_back(cauchy); + + // Tukey noise model + auto tukey = noiseModel::Robust::Create( + noiseModel::mEstimator::Tukey::Create(4.6851), gaussian); + robust_models.push_back(tukey); + + // Welsch noise model + auto welsch = noiseModel::Robust::Create( + noiseModel::mEstimator::Welsch::Create(2.9846), gaussian); + robust_models.push_back(welsch); + + // Geman-McClure noise model + auto gm = noiseModel::Robust::Create( + noiseModel::mEstimator::GemanMcClure::Create(1.0), gaussian); + robust_models.push_back(gm); + + // DCS noise model + auto dcs = noiseModel::Robust::Create( + noiseModel::mEstimator::DCS::Create(1.0), gaussian); + robust_models.push_back(dcs); + + // L2WithDeadZone noise model + auto l2 = noiseModel::Robust::Create( + noiseModel::mEstimator::L2WithDeadZone::Create(1.0), gaussian); + robust_models.push_back(l2); + + for(auto&& model: robust_models) { + factor = PriorFactor(X(3), prior, model); + values.clear(); + values.insert(X(3), estimate); + CHECK(assert_equal(model->robust()->weight(distance), factor.weight(values))); + } +} + /* ************************************************************************* */ TEST( NonlinearFactor, linearize_f1 ) { diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 2e99aff71..496122419 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -114,94 +114,94 @@ using symbol_shorthand::L; /* Create GUIDs for Noisemodels */ /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") /* Create GUIDs for geometry */ /* ************************************************************************* */ -GTSAM_VALUE_EXPORT(gtsam::Point2); -GTSAM_VALUE_EXPORT(gtsam::StereoPoint2); -GTSAM_VALUE_EXPORT(gtsam::Point3); -GTSAM_VALUE_EXPORT(gtsam::Rot2); -GTSAM_VALUE_EXPORT(gtsam::Rot3); -GTSAM_VALUE_EXPORT(gtsam::Pose2); -GTSAM_VALUE_EXPORT(gtsam::Pose3); -GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); -GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); -GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); -GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); -GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2); -GTSAM_VALUE_EXPORT(gtsam::StereoCamera); +GTSAM_VALUE_EXPORT(gtsam::Point2) +GTSAM_VALUE_EXPORT(gtsam::StereoPoint2) +GTSAM_VALUE_EXPORT(gtsam::Point3) +GTSAM_VALUE_EXPORT(gtsam::Rot2) +GTSAM_VALUE_EXPORT(gtsam::Rot3) +GTSAM_VALUE_EXPORT(gtsam::Pose2) +GTSAM_VALUE_EXPORT(gtsam::Pose3) +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2) +GTSAM_VALUE_EXPORT(gtsam::Cal3DS2) +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo) +GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera) +GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2) +GTSAM_VALUE_EXPORT(gtsam::StereoCamera) /* Create GUIDs for factors */ /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor") -BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3"); -BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3"); -BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3"); -BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2") +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2") +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3") +BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2") +BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3") +BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2") +BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3") +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2") +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2") +BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera") +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera") -BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2") +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3") +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2") +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3") +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2") +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3") -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera") -BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D"); -BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); -BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point"); -BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2"); +BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D") +BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D") +BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2") +BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3") +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint") +BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point") +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera") +BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2") -BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); +BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D") -BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); -BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2") +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2") -BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); -BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2") +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2") -BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2") -BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); +BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D") /* ************************************************************************* */ diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index fb9f7a5a2..84ccc131a 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -200,7 +200,7 @@ TEST(SubgraphPreconditioner, system) { } /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor") // Read from XML file static GaussianFactorGraph read(const string& name) {