12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'develop' into feature/sphericalCamera

release/4.3a0
lcarlone 2021-12-04 18:51:16 -05:00
parent 95b26742ee cbf4f9f3fd
commit 260312af38
111 changed files with 2306 additions and 1952 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -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

12
examples/DiscreteBayesNetExample.cpp
View File

@ -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;
}

16
examples/DiscreteBayesNet_FG.cpp
View File

@ -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<bool>((*values)[C])
<< " Sprinkler = " << static_cast<bool>((*values)[S])
<< " Rain = " << boolalpha << static_cast<bool>((*values)[R])
<< " WetGrass = " << static_cast<bool>((*values)[W]) << endl;
auto print = [=](const DiscreteFactor::Values& values) {
cout << boolalpha << "Cloudy = " << static_cast<bool>(values.at(C))
<< " Sprinkler = " << static_cast<bool>(values.at(S))
<< " Rain = " << boolalpha << static_cast<bool>(values.at(R))
<< " WetGrass = " << static_cast<bool>(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;

8
examples/HMMExample.cpp
View File

@ -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

4
examples/UGM_chain.cpp
View File

@ -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

4
examples/UGM_small.cpp
View File

@ -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;

2
gtsam/base/Lie.h
View File

@ -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<T>;
#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::IsLieGroup<T> _gtsam_IsLieGroup_##T;
#define GTSAM_CONCEPT_LIE_TYPE(T) using _gtsam_IsLieGroup_##T = gtsam::IsLieGroup<T>;

2
gtsam/base/Manifold.h
View File

@ -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<T>;
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::IsManifold<T> _gtsam_IsManifold_##T;
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold<T>;

38
gtsam/base/Matrix.h
View File

@ -46,28 +46,28 @@ typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> 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<double, N, N> Matrix##N; \
typedef Eigen::Matrix<double, 1, N> Matrix1##N; \
typedef Eigen::Matrix<double, 2, N> Matrix2##N; \
typedef Eigen::Matrix<double, 3, N> Matrix3##N; \
typedef Eigen::Matrix<double, 4, N> Matrix4##N; \
typedef Eigen::Matrix<double, 5, N> Matrix5##N; \
typedef Eigen::Matrix<double, 6, N> Matrix6##N; \
typedef Eigen::Matrix<double, 7, N> Matrix7##N; \
typedef Eigen::Matrix<double, 8, N> Matrix8##N; \
typedef Eigen::Matrix<double, 9, N> Matrix9##N; \
using Matrix##N = Eigen::Matrix<double, N, N>; \
using Matrix1##N = Eigen::Matrix<double, 1, N>; \
using Matrix2##N = Eigen::Matrix<double, 2, N>; \
using Matrix3##N = Eigen::Matrix<double, 3, N>; \
using Matrix4##N = Eigen::Matrix<double, 4, N>; \
using Matrix5##N = Eigen::Matrix<double, 5, N>; \
using Matrix6##N = Eigen::Matrix<double, 6, N>; \
using Matrix7##N = Eigen::Matrix<double, 7, N>; \
using Matrix8##N = Eigen::Matrix<double, 8, N>; \
using Matrix9##N = Eigen::Matrix<double, 9, N>; \
static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
static const Eigen::MatrixBase<Matrix##N>::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<Matrix> SubMatrix;

2
gtsam/base/Testable.h
View File

@ -173,4 +173,4 @@ namespace gtsam {
* @deprecated please use BOOST_CONCEPT_ASSERT and
*/
#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable<T>;
#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::IsTestable<T> _gtsam_Testable_##T;
#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable<T>;

2
gtsam/base/TestableAssertions.h
View File

@ -85,7 +85,7 @@ bool assert_equal(const V& expected, const boost::optional<const V&>& actual, do
* \deprecated: use container equals instead
*/
template<class V>
bool assert_equal(const std::vector<V>& expected, const std::vector<V>& actual, double tol = 1e-9) {
bool GTSAM_DEPRECATED assert_equal(const std::vector<V>& expected, const std::vector<V>& actual, double tol = 1e-9) {
bool match = true;
if (expected.size() != actual.size())
match = false;

24
gtsam/base/Vector.h
View File

@ -48,18 +48,18 @@ static const Eigen::MatrixBase<Vector3>::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<double, N, 1> Vector##N; \
using Vector##N = Eigen::Matrix<double, N, 1>; \
static const Eigen::MatrixBase<Vector##N>::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<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> 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<class V1, class V2>
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;
}

2
gtsam/base/chartTesting.h
View File

@ -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<T> Chart;
typedef typename Chart::vector Vector;

14
gtsam/base/serialization.h
View File

@ -19,8 +19,9 @@
#pragma once
#include <sstream>
#include <Eigen/Core>
#include <fstream>
#include <sstream>
#include <string>
// includes for standard serialization types
@ -40,6 +41,17 @@
#include <boost/archive/binary_oarchive.hpp>
#include <boost/serialization/export.hpp>
// 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<boost::serialization::U> {enum {Flags=0};};
} }
#endif
#endif
namespace gtsam {
/** @name Standard serialization

4
gtsam/base/tests/testVector.cpp
View File

@ -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)));

8
gtsam/base/types.h
View File

@ -34,6 +34,14 @@
#include <tbb/scalable_allocator.h>
#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 <omp.h>
#endif

12
gtsam/discrete/DiscreteBayesNet.cpp
View File

@ -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;
}

4
gtsam/discrete/DiscreteBayesNet.h
View File

@ -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;
///@}

14
gtsam/discrete/DiscreteConditional.cpp
View File

@ -35,7 +35,7 @@ using namespace std;
namespace gtsam {
// Instantiate base class
template class Conditional<DecisionTreeFactor, DiscreteConditional> ;
template class GTSAM_EXPORT Conditional<DecisionTreeFactor, DiscreteConditional> ;
/* ******************************************************************************** */
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
}
/* ******************************************************************************** */

5
gtsam/discrete/DiscreteConditional.h
View File

@ -45,7 +45,6 @@ public:
/** A map from keys to values..
* TODO: Again, do we need this??? */
typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> 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;
/// @}

1
gtsam/discrete/DiscreteFactor.h
View File

@ -51,7 +51,6 @@ public:
* the new class DiscreteValue, as the varible's type (domain)
*/
typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> sharedValues;
public:

2
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -94,7 +94,7 @@ namespace gtsam {
// }
/* ************************************************************************* */
DiscreteFactor::sharedValues DiscreteFactorGraph::optimize() const
DiscreteFactorGraph::Values DiscreteFactorGraph::optimize() const
{
gttic(DiscreteFactorGraph_optimize);
return BaseEliminateable::eliminateSequential()->optimize();

19
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -74,7 +74,6 @@ public:
/** A map from keys to values */
typedef KeyVector Indices;
typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> sharedValues;
/** Default constructor */
DiscreteFactorGraph() {}
@ -101,25 +100,27 @@ public:
/// @}
template<class SOURCE>
// Add single key decision-tree factor.
template <class SOURCE>
void add(const DiscreteKey& j, SOURCE table) {
DiscreteKeys keys;
keys.push_back(j);
push_back(boost::make_shared<DecisionTreeFactor>(keys, table));
emplace_shared<DecisionTreeFactor>(keys, table);
}
template<class SOURCE>
// Add binary key decision-tree factor.
template <class SOURCE>
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<DecisionTreeFactor>(keys, table));
emplace_shared<DecisionTreeFactor>(keys, table);
}
/** add shared discreteFactor immediately from arguments */
template<class SOURCE>
// Add shared discreteFactor immediately from arguments.
template <class SOURCE>
void add(const DiscreteKeys& keys, SOURCE table) {
push_back(boost::make_shared<DecisionTreeFactor>(keys, table));
emplace_shared<DecisionTreeFactor>(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 */

12
gtsam/discrete/tests/testDiscreteBayesNet.cpp
View File

@ -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));
}
/* ************************************************************************* */

16
gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View File

@ -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();

46
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -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;

2
gtsam/geometry/PinholePose.h
View File

@ -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<CALIBRATION>::value;

2
gtsam/geometry/Pose2.cpp
View File

@ -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.));

14
gtsam/geometry/Pose3.cpp
View File

@ -30,7 +30,7 @@ using std::vector;
using Point3Pairs = vector<Point3Pair>;
/** 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;
}
/* ************************************************************************* */

10
gtsam/geometry/Pose3.h
View File

@ -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);

24
gtsam/geometry/Quaternion.h
View File

@ -117,13 +117,23 @@ struct traits<QUATERNION_TYPE> {
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<double>());

4
gtsam/geometry/Similarity3.cpp
View File

@ -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) {

2
gtsam/geometry/concepts.h
View File

@ -72,5 +72,5 @@ private:
/** Pose Concept macros */
#define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept<T>;
#define GTSAM_CONCEPT_POSE_TYPE(T) typedef gtsam::PoseConcept<T> _gtsam_PoseConcept##T;
#define GTSAM_CONCEPT_POSE_TYPE(T) using _gtsam_PoseConcept##T = gtsam::PoseConcept<T>;

3
gtsam/geometry/geometry.i
View File

@ -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);

27
gtsam/geometry/tests/testPose3.cpp
View File

@ -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<Vector6, Vector6, Vector6>(
testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5);
Matrix numericalH1 = numericalDerivative21<Vector6, Vector6, Vector6>(
testDerivAdjoint, screwPose3::xi, v, 1e-5);
Matrix numericalH2 = numericalDerivative22<Vector6, Vector6, Vector6>(
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<Vector6, Vector6, Vector6>(
Matrix numericalH1 = numericalDerivative21<Vector6, Vector6, Vector6>(
testDerivAdjointTranspose, xi, v, 1e-5);
Matrix numericalH2 = numericalDerivative22<Vector6, Vector6, Vector6>(
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));
}
/* ************************************************************************* */

2
gtsam/inference/BayesTree.h
View File

@ -19,6 +19,8 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/ConcurrentMap.h>

2
gtsam/inference/ClusterTree.h
View File

@ -110,7 +110,7 @@ class ClusterTree {
typedef sharedCluster sharedNode;
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType)
protected:
FastVector<sharedNode> roots_;

48
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -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<class FACTORGRAPH>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::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 <class FACTORGRAPH>
boost::shared_ptr<
typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::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);
}
}
}

14
gtsam/inference/EliminateableFactorGraph.h
View File

@ -288,8 +288,9 @@ namespace gtsam {
FactorGraphType& asDerived() { return static_cast<FactorGraphType&>(*this); }
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated ordering and orderingType shouldn't both be specified */
boost::shared_ptr<BayesNetType> eliminateSequential(
boost::shared_ptr<BayesNetType> 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<BayesNetType> eliminateSequential(
boost::shared_ptr<BayesNetType> 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<BayesTreeType> eliminateMultifrontal(
boost::shared_ptr<BayesTreeType> 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<BayesTreeType> eliminateMultifrontal(
boost::shared_ptr<BayesTreeType> 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<BayesNetType> marginalMultifrontalBayesNet(
boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> variables,
boost::none_t,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
@ -332,13 +333,14 @@ namespace gtsam {
}
/** \deprecated */
boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> variables,
boost::none_t,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const {
return marginalMultifrontalBayesTree(variables, function, variableIndex);
}
#endif
};
}

2
gtsam/inference/EliminationTree.h
View File

@ -81,7 +81,7 @@ namespace gtsam {
protected:
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType)
FastVector<sharedNode> roots_;
FastVector<sharedFactor> remainingFactors_;

1
gtsam/inference/Factor.h
View File

@ -22,6 +22,7 @@
#pragma once
#include <boost/serialization/nvp.hpp>
#include <boost/shared_ptr.hpp>
#include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>

4
gtsam/linear/Errors.cpp
View File

@ -111,10 +111,10 @@ double dot(const Errors& a, const Errors& b) {
/* ************************************************************************* */
template<>
void axpy<Errors,Errors>(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++));
}
/* ************************************************************************* */

2
gtsam/linear/Errors.h
View File

@ -66,7 +66,7 @@ namespace gtsam {
* BLAS level 2 style
*/
template <>
GTSAM_EXPORT void axpy<Errors,Errors>(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");

13
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -19,7 +19,6 @@
*/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/linear/GaussianJunctionTree.h>
@ -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,

18
gtsam/linear/GaussianFactorGraph.h
View File

@ -21,12 +21,13 @@
#pragma once
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
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
};

54
gtsam/linear/NoiseModel.h
View File

@ -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<Matrix>& sqrt_information = boost::none) :
Base(dim), sqrt_information_(sqrt_information) {
}
public:
typedef boost::shared_ptr<Gaussian> shared_ptr;
/** constructor takes square root information matrix */
Gaussian(size_t dim = 1,
const boost::optional<Matrix>& 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<Diagonal> 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<Constrained> 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<Isotropic> 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<Unit> 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<Matrix>& 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());
}

4
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -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
}
/* ************************************************************************* */

4
gtsam/linear/iterative-inl.h
View File

@ -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;

18
gtsam/linear/linear.i
View File

@ -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;

2
gtsam/linear/tests/testErrors.cpp
View File

@ -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));

31
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -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);
}

20
gtsam/linear/tests/testSerializationLinear.cpp
View File

@ -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) {

2
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -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)

2
gtsam/navigation/CombinedImuFactor.h
View File

@ -353,4 +353,4 @@ struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
} // namespace gtsam
/// Add Boost serialization export key (declaration) for derived class
BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams);
BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams)

24
gtsam/navigation/navigation.i
View File

@ -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 <gtsam/navigation/PreintegratedRotation.h>
@ -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 <gtsam/navigation/ImuFactor.h>
@ -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 {

2
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -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) {

4
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -72,7 +72,7 @@ TEST( GPSFactor, Constructor ) {
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Pose3>(
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<Vector,NavState>(
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;

14
gtsam/navigation/tests/testImuFactorSerialization.cpp
View File

@ -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 <typename P>
P getPreintegratedMeasurements() {

16
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -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<Point3,Rot2> //
(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<Vector,Rot2> //
(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<Vector,Rot3> //
(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<Vector,Point3> //
(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<Vector,Point3> //
(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<Vector,double> //
(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<Vector,Unit3> //
(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<Vector,Point3> //
(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));
}

2
gtsam/nonlinear/CustomFactor.h
View File

@ -101,4 +101,4 @@ private:
}
};
};
}

3
gtsam/nonlinear/DoglegOptimizerImpl.cpp
View File

@ -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;
}

2
gtsam/nonlinear/ExpressionFactor.h
View File

@ -305,7 +305,7 @@ struct traits<ExpressionFactorN<T, Args...>>
* \deprecated Prefer the more general ExpressionFactorN<>.
*/
template <typename T, typename A1, typename A2>
class ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
public:
/// Destructor
~ExpressionFactor2() override {}

5
gtsam/nonlinear/GncOptimizer.h
View File

@ -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;

6
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -29,9 +29,6 @@ protected:
GaussianFactor::shared_ptr factor_;
boost::optional<Values> linearizationPoint_;
/** Default constructor - necessary for serialization */
LinearContainerFactor() {}
/** direct copy constructor */
GTSAM_EXPORT LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional<Values>& linearizationPoint);
@ -43,6 +40,9 @@ public:
typedef boost::shared_ptr<This> 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());

12
gtsam/nonlinear/Marginals.cpp
View File

@ -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);
}
/* ************************************************************************* */

8
gtsam/nonlinear/Marginals.h
View File

@ -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<Ordering> */
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<Ordering> */
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<Ordering> */
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
};

2
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View File

@ -149,7 +149,7 @@ boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
const V &initial, const NonlinearOptimizerParams &params,
const bool singleIteration, const bool gradientDescent = false) {
// GTSAM_CONCEPT_MANIFOLD_TYPE(V);
// GTSAM_CONCEPT_MANIFOLD_TYPE(V)
size_t iteration = 0;

1
gtsam/nonlinear/NonlinearEquality.h
View File

@ -219,7 +219,6 @@ protected:
X value_; /// fixed value for variable
GTSAM_CONCEPT_MANIFOLD_TYPE(X)
GTSAM_CONCEPT_TESTABLE_TYPE(X)
public:

2
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -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;

6
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -265,15 +265,17 @@ namespace gtsam {
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated */
boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
boost::shared_ptr<HessianFactor> 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
};

10
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -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)

12
gtsam/nonlinear/Values.h
View File

@ -24,6 +24,7 @@
#pragma once
#include <gtsam/base/FastDefaultAllocator.h>
#include <gtsam/base/GenericValue.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/inference/Key.h>
@ -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<typename std::pair<const Key, void*>>::type KeyValuePtrPairAllocator;
typedef boost::ptr_map<
Key,
Value,
std::less<Key>,
ValueCloneAllocator,
boost::fast_pool_allocator<std::pair<const Key, void*> > > KeyValueMap;
KeyValuePtrPairAllocator > KeyValueMap;
// The member to store the values, see just above
KeyValueMap values_;

11
gtsam/nonlinear/factorTesting.h
View File

@ -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<std::pair<Key, Matrix> > 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

16
gtsam/nonlinear/nonlinear.i
View File

@ -115,6 +115,10 @@ class Ordering {
Ordering();
Ordering(const gtsam::Ordering& other);
template <FACTOR_GRAPH = {gtsam::NonlinearFactorGraph,
gtsam::GaussianFactorGraph}>
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 <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
@ -744,12 +753,17 @@ class ISAM2 {
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>, 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 <gtsam/nonlinear/NonlinearISAM.h>

48
gtsam/nonlinear/tests/testSerializationNonlinear.cpp
View File

@ -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::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>");
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::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>")
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<Cal3_S2>);
GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3DS2>);
GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3Bundler>);
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<Cal3_S2>)
GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
namespace detail {
template<class T> struct pack {

6
gtsam/slam/GeneralSFMFactor.h
View File

@ -59,8 +59,8 @@ namespace gtsam {
template<class CAMERA, class LANDMARK>
class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
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<CAMERA>::value;
static const int DimL = FixedDimension<LANDMARK>::value;
@ -202,7 +202,7 @@ struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
template<class CALIBRATION>
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION);
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
static const int DimK = FixedDimension<CALIBRATION>::value;
protected:

2
gtsam/slam/dataset.cpp
View File

@ -392,7 +392,7 @@ parseMeasurements(const std::string &filename,
size_t maxIndex) {
ParseMeasurement<Pose2> parse{model ? createSampler(model) : nullptr,
maxIndex, true, NoiseFormatAUTO,
KernelFunctionTypeNONE};
KernelFunctionTypeNONE, nullptr};
return parseToVector<BinaryMeasurement<Pose2>>(filename, parse);
}

8
gtsam/slam/slam.i
View File

@ -168,6 +168,13 @@ template <POSE>
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<gtsam::Pose2> PoseTranslationPrior2D;
@ -178,6 +185,7 @@ template <POSE>
virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
PoseRotationPrior(size_t key, const POSE& pose_z,
const gtsam::noiseModel::Base* noiseModel);
POSE measured() const;
};
typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;

14
gtsam/slam/tests/testSmartFactorBase.cpp
View File

@ -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;

14
gtsam/slam/tests/testSmartProjectionFactor.cpp
View File

@ -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;

14
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -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;

178
gtsam_unstable/discrete/AllDiff.cpp
View File

@ -5,107 +5,109 @@
* @author Frank Dellaert
*/
#include <gtsam_unstable/discrete/Domain.h>
#include <gtsam_unstable/discrete/AllDiff.h>
#include <gtsam/base/Testable.h>
#include <gtsam_unstable/discrete/AllDiff.h>
#include <gtsam_unstable/discrete/Domain.h>
#include <boost/make_shared.hpp>
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<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
}
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<Domain> 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<Domain>& 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<AllDiff>(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<AllDiff>(newKeys);
}
/* ************************************************************************* */
Constraint::shared_ptr AllDiff::partiallyApply(
const std::vector<Domain>& 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

115
gtsam_unstable/discrete/AllDiff.h
View File

@ -7,71 +7,66 @@
#pragma once
#include <gtsam_unstable/discrete/BinaryAllDiff.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam_unstable/discrete/BinaryAllDiff.h>
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<Key, size_t> 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<const AllDiff*>(&other))
return false;
else {
const AllDiff& f(static_cast<const AllDiff&>(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<Key,size_t> 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<const AllDiff*>(&other))
return false;
else {
const AllDiff& f(static_cast<const AllDiff&>(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<Domain>& 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<Domain>&) const override;
};
} // namespace gtsam
} // namespace gtsam

154
gtsam_unstable/discrete/BinaryAllDiff.h
View File

@ -7,94 +7,90 @@
#pragma once
#include <gtsam_unstable/discrete/Domain.h>
#include <gtsam_unstable/discrete/Constraint.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam_unstable/discrete/Constraint.h>
#include <gtsam_unstable/discrete/Domain.h>
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<const BinaryAllDiff*>(&other))
return false;
else {
const BinaryAllDiff& f(static_cast<const BinaryAllDiff&>(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<double> 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<Domain>& domains) const override {
// throw std::runtime_error(
// "BinaryAllDiff::ensureArcConsistency not implemented");
/// equals
bool equals(const DiscreteFactor& other, double tol) const override {
if (!dynamic_cast<const BinaryAllDiff*>(&other))
return false;
else {
const BinaryAllDiff& f(static_cast<const BinaryAllDiff&>(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<Domain>&) 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<double> 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

160
gtsam_unstable/discrete/CSP.cpp
View File

@ -5,99 +5,95 @@
* @author Frank Dellaert
*/
#include <gtsam_unstable/discrete/Domain.h>
#include <gtsam_unstable/discrete/CSP.h>
#include <gtsam/base/Testable.h>
#include <gtsam_unstable/discrete/CSP.h>
#include <gtsam_unstable/discrete/Domain.h>
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<bool> 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<Constraint>((*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<Index> dkeys;
for(const DiscreteFactor::shared_ptr& f: factors_) {
Constraint::shared_ptr constraint = boost::dynamic_pointer_cast<Constraint>(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<Constraint>((*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<Index> dkeys;
CSP new_csp;
// Add tightened domains as new factors:
for (auto key_domain : domains) {
new_csp.emplace_shared<Domain>(key_domain.second);
}
// Reduce all existing factors:
for (const DiscreteFactor::shared_ptr& f : factors_) {
auto constraint = boost::dynamic_pointer_cast<Constraint>(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

138
gtsam_unstable/discrete/CSP.h
View File

@ -7,84 +7,78 @@
#pragma once
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam_unstable/discrete/AllDiff.h>
#include <gtsam_unstable/discrete/SingleValue.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
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<Key> Values;
public:
/// Add a unary constraint, allowing only a single value
void addSingleValue(const DiscreteKey& dkey, size_t value) {
emplace_shared<SingleValue>(dkey, value);
}
/// Add a binary AllDiff constraint
void addAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) {
emplace_shared<BinaryAllDiff>(key1, key2);
}
/// Add a general AllDiff constraint
void addAllDiff(const DiscreteKeys& dkeys) { emplace_shared<AllDiff>(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<Key> Values;
typedef boost::shared_ptr<Values> 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<SingleValue> factor(new SingleValue(dkey, value));
push_back(factor);
}
/// Add a binary AllDiff constraint
void addAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) {
boost::shared_ptr<BinaryAllDiff> factor(
new BinaryAllDiff(key1, key2));
push_back(factor);
}
/// Add a general AllDiff constraint
void addAllDiff(const DiscreteKeys& dkeys) {
boost::shared_ptr<AllDiff> 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

119
gtsam_unstable/discrete/Constraint.h
View File

@ -17,77 +17,70 @@
#pragma once
#include <gtsam_unstable/dllexport.h>
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam_unstable/dllexport.h>
#include <boost/assign.hpp>
#include <map>
namespace gtsam {
class Domain;
class Domain;
using Domains = std::map<Key, Domain>;
/**
* 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<Constraint> 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 <class KeyIterator>
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<Constraint> 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<class KeyIterator>
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<Domain>& 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<Domain>&) const = 0;
/// @}
};
/// Partially apply known values, domain version
virtual shared_ptr partiallyApply(const Domains&) const = 0;
/// @}
};
// DiscreteFactor
}// namespace gtsam
} // namespace gtsam

168
gtsam_unstable/discrete/Domain.cpp
View File

@ -5,92 +5,94 @@
* @author Frank Dellaert
*/
#include <gtsam_unstable/discrete/Domain.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/base/Testable.h>
#include <boost/make_shared.hpp>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam_unstable/discrete/Domain.h>
#include <boost/make_shared.hpp>
#include <sstream>
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<double> 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<Domain>& 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<Domain>& 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<Domain>& 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<double> 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> 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<Domain>(*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<Domain>(Dk);
}
/* ************************************************************************* */
} // namespace gtsam

194
gtsam_unstable/discrete/Domain.h
View File

@ -7,111 +7,107 @@
#pragma once
#include <gtsam_unstable/discrete/Constraint.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam_unstable/discrete/Constraint.h>
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<size_t> values_; /// allowed values
public:
typedef boost::shared_ptr<Domain> 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<const Domain*>(&other))
return false;
else {
const Domain& f(static_cast<const Domain&>(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<size_t> 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<Domain> 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<Domain> 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<const Domain*>(&other))
return false;
else {
const Domain& f(static_cast<const Domain&>(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<Domain>& 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<Domain>& 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<Domain>& domains) const override;
};
} // namespace gtsam
} // namespace gtsam

509
gtsam_unstable/discrete/Scheduler.cpp
View File

@ -5,298 +5,285 @@
* @author Frank Dellaert
*/
#include <gtsam_unstable/discrete/Scheduler.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam_unstable/discrete/Scheduler.h>
#include <boost/tokenizer.hpp>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <cmath>
namespace gtsam {
using namespace std;
using namespace std;
Scheduler::Scheduler(size_t maxNrStudents, const string& filename):
maxNrStudents_(maxNrStudents)
{
typedef boost::tokenizer<boost::escaped_list_separator<char> > Tokenizer;
Scheduler::Scheduler(size_t maxNrStudents, const string& filename)
: maxNrStudents_(maxNrStudents) {
typedef boost::tokenizer<boost::escaped_list_separator<char> > 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()<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<size_t> 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<nrStudents());
return students_[i].name_;
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);
}
const DiscreteKey& Scheduler::studentKey(size_t i) const {
assert(i<nrStudents());
return students_[i].key_;
// 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() < 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<size_t> 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<size_t> 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<nrStudents());
return students_[i].areaName_[area];
}
// For all areas
for (size_t area = 0; area < 3; area++) {
DiscreteKey areaKey = s.keys_[area];
const string& areaName = s.areaName_[area];
/** Add student-specific constraints to the graph */
void Scheduler::addStudentSpecificConstraints(size_t i, boost::optional<size_t> 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(i<nrStudents());
const Student& s = students_[i];
if (debug) cout << "Advisor constraint " << areaName << endl;
assert(s.advisor_.size() == areaKey.second);
CSP::add(areaKey, s.advisor_);
if (!slot && !slotsAvailable_.empty()) {
if (debug) cout << "Adding availability of slots" << endl;
assert(slotsAvailable_.size()==s.key_.second);
CSP::add(s.key_, slotsAvailable_);
}
// For all areas
for (size_t area = 0; area < 3; area++) {
DiscreteKey areaKey = s.keys_[area];
const string& areaName = s.areaName_[area];
if (debug) cout << "Area constraints " << areaName << endl;
assert(facultyInArea_[areaName].size()==areaKey.second);
CSP::add(areaKey, facultyInArea_[areaName]);
if (debug) cout << "Advisor constraint " << areaName << endl;
assert(s.advisor_.size()==areaKey.second);
CSP::add(areaKey, s.advisor_);
if (debug) cout << "Availability of faculty " << areaName << endl;
if (slot) {
// get all constraints then specialize to slot
size_t dummyIndex = maxNrStudents_*3+maxNrStudents_;
DiscreteKey dummy(dummyIndex, nrTimeSlots());
Potentials::ADT p(dummy & areaKey, available_); // available_ is Doodle string
Potentials::ADT q = p.choose(dummyIndex, *slot);
DiscreteFactor::shared_ptr f(new DecisionTreeFactor(areaKey, q));
CSP::push_back(f);
} else {
CSP::add(s.key_, areaKey, available_); // available_ is Doodle string
}
}
// add mutex
if (debug) cout << "Mutex for faculty" << endl;
addAllDiff(s.keys_[0] & s.keys_[1] & s.keys_[2]);
}
/** Main routine that builds factor graph */
void Scheduler::buildGraph(size_t mutexBound) {
bool debug = ISDEBUG("Scheduler::buildGraph");
if (debug) cout << "Adding student-specific constraints" << endl;
for (size_t i = 0; i < nrStudents(); i++)
addStudentSpecificConstraints(i);
// special constraint for MN
if (studentName(0) == "Michael N") CSP::add(studentKey(0),
"0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1");
if (!mutexBound) {
DiscreteKeys dkeys;
for(const Student& s: students_)
dkeys.push_back(s.key_);
addAllDiff(dkeys);
if (debug) cout << "Availability of faculty " << areaName << endl;
if (slot) {
// get all constraints then specialize to slot
size_t dummyIndex = maxNrStudents_ * 3 + maxNrStudents_;
DiscreteKey dummy(dummyIndex, nrTimeSlots());
Potentials::ADT p(dummy & areaKey,
available_); // available_ is Doodle string
Potentials::ADT q = p.choose(dummyIndex, *slot);
DiscreteFactor::shared_ptr f(new DecisionTreeFactor(areaKey, q));
CSP::push_back(f);
} else {
if (debug) cout << "Mutex for Students" << endl;
for (size_t i1 = 0; i1 < nrStudents(); i1++) {
// if mutexBound=1, we only mutex with next student
size_t bound = min((i1 + 1 + mutexBound), nrStudents());
for (size_t i2 = i1 + 1; i2 < bound; i2++) {
addAllDiff(studentKey(i1), studentKey(i2));
}
CSP::add(s.key_, areaKey, available_); // available_ is Doodle string
}
}
// add mutex
if (debug) cout << "Mutex for faculty" << endl;
addAllDiff(s.keys_[0] & s.keys_[1] & s.keys_[2]);
}
/** Main routine that builds factor graph */
void Scheduler::buildGraph(size_t mutexBound) {
bool debug = ISDEBUG("Scheduler::buildGraph");
if (debug) cout << "Adding student-specific constraints" << endl;
for (size_t i = 0; i < nrStudents(); i++) addStudentSpecificConstraints(i);
// special constraint for MN
if (studentName(0) == "Michael N")
CSP::add(studentKey(0), "0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1");
if (!mutexBound) {
DiscreteKeys dkeys;
for (const Student& s : students_) dkeys.push_back(s.key_);
addAllDiff(dkeys);
} else {
if (debug) cout << "Mutex for Students" << endl;
for (size_t i1 = 0; i1 < nrStudents(); i1++) {
// if mutexBound=1, we only mutex with next student
size_t bound = min((i1 + 1 + mutexBound), nrStudents());
for (size_t i2 = i1 + 1; i2 < bound; i2++) {
addAllDiff(studentKey(i1), studentKey(i2));
}
}
} // buildGraph
/** 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(sharedValues 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;
}
}
} // 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(f<stats.size());
stats[f]++;
} // area
} // s
/** Special print for single-student case */
void Scheduler::printSpecial(const Values& 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;
}
cout << endl;
}
/** Accumulate faculty stats */
void Scheduler::accumulateStats(const Values& 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(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; i<maxKey; ++i)
defaultKeyOrdering += Key(i);
DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(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

253
gtsam_unstable/discrete/Scheduler.h
View File

@ -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<DiscreteKey> keys_; // key for areas
std::vector<std::string> areaName_;
std::vector<double> 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<Student> students_;
/** faculty identifiers */
std::map<std::string, size_t> facultyIndex_;
std::vector<std::string> facultyName_, slotName_, areaName_;
/** area constraints */
typedef std::map<std::string, std::vector<double> > FacultyInArea;
FacultyInArea facultyInArea_;
/** nrTimeSlots * nrFaculty availability constraints */
std::string available_;
/** which slots are good */
std::vector<double> 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<DiscreteKey> keys_; // key for areas
std::vector<std::string> areaName_;
std::vector<double> 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<Student> students_;
/** boolean std::string of nrTimeSlots * nrFaculty */
void setAvailability(const std::string& available) { available_ = available; }
/** faculty identifiers */
std::map<std::string, size_t> facultyIndex_;
std::vector<std::string> facultyName_, slotName_, areaName_;
void addSlot(const std::string& slotName) { slotName_.push_back(slotName); }
/** area constraints */
typedef std::map<std::string, std::vector<double> > 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<double> slotsAvailable_;
/** slots available, boolean */
void setSlotsAvailable(const std::vector<double>& slotsAvailable) {
slotsAvailable_ = slotsAvailable;
}
public:
void addArea(const std::string& facultyName, const std::string& areaName) {
areaName_.push_back(areaName);
std::vector<double>& 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<size_t> 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<size_t> 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<double>& 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<double>& 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<size_t>& stats) const;
/** get key for student and area, 0 is time slot itself */
const DiscreteKey& key(size_t s, boost::optional<size_t> 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<size_t> 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<size_t>& 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

128
gtsam_unstable/discrete/SingleValue.cpp
View File

@ -5,75 +5,73 @@
* @author Frank Dellaert
*/
#include <gtsam_unstable/discrete/SingleValue.h>
#include <gtsam_unstable/discrete/Domain.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam_unstable/discrete/Domain.h>
#include <gtsam_unstable/discrete/SingleValue.h>
#include <boost/make_shared.hpp>
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<double> 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<Domain>& 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<Domain>& 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<double> 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<SingleValue>(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<SingleValue>(discreteKey(), value_);
}
/* ************************************************************************* */
} // namespace gtsam

125
gtsam_unstable/discrete/SingleValue.h
View File

@ -7,76 +7,71 @@
#pragma once
#include <gtsam_unstable/discrete/Constraint.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam_unstable/discrete/Constraint.h>
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<SingleValue> 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<const SingleValue*>(&other))
return false;
else {
const SingleValue& f(static_cast<const SingleValue&>(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<SingleValue> 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<const SingleValue*>(&other))
return false;
else {
const SingleValue& f(static_cast<const SingleValue&>(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<Domain>& 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<Domain>& domains) const override;
};
} // namespace gtsam
} // namespace gtsam

6
gtsam_unstable/discrete/examples/schedulingExample.cpp
View File

@ -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<size_t> stats(19, 0);
vector<Scheduler::sharedValues> samples;
vector<Scheduler::Values> 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);
}
}

6
gtsam_unstable/discrete/examples/schedulingQuals12.cpp
View File

@ -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<size_t> 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<size_t> stats(19, 0);
vector<Scheduler::sharedValues> samples;
vector<Scheduler::Values> samples;
for (size_t i = 0; i < NRSTUDENTS; i++) {
samples.push_back(samplers[i]->sample());
schedulers[i].accumulateStats(samples[i], stats);

6
gtsam_unstable/discrete/examples/schedulingQuals13.cpp
View File

@ -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<size_t> 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<size_t> stats(nrFaculty, 0);
vector<Scheduler::sharedValues> samples;
vector<Scheduler::Values> samples;
for (size_t i = 0; i < NRSTUDENTS; i++) {
samples.push_back(samplers[i]->sample());
schedulers[i].accumulateStats(samples[i], stats);

252
gtsam_unstable/discrete/tests/testCSP.cpp
View File

@ -7,49 +7,109 @@
#include <gtsam_unstable/discrete/CSP.h>
#include <gtsam_unstable/discrete/Domain.h>
#include <boost/assign/std/map.hpp>
using boost::assign::insert;
#include <CppUnitLite/TestHarness.h>
#include <iostream>
#include <fstream>
#include <iostream>
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<DiscreteKey> 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<DiscreteKey> dkeys;
dkeys += ID,UT,AZ;
vector<DiscreteKey> 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<Domain> 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);
}
/* ************************************************************************* */

123
gtsam_unstable/discrete/tests/testLoopyBelief.cpp
View File

@ -5,14 +5,15 @@
* @date Oct 11, 2013
*/
#include <gtsam/inference/VariableIndex.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/range/adaptor/map.hpp>
#include <gtsam/inference/VariableIndex.h>
#include <boost/assign/list_of.hpp>
#include <iostream>
#include <boost/range/adaptor/map.hpp>
#include <fstream>
#include <iostream>
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<Key, size_t> 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<Key, StarGraph> 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<Key, DiscreteKey>& allDiscreteKeys) :
starGraphs_(buildStarGraphs(graph, allDiscreteKeys)) {
}
const std::map<Key, DiscreteKey>& 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<Key, DiscreteKey>& 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<Key, std::map<Key, DiscreteFactor::shared_ptr> > 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<Key, DiscreteFactor::shared_ptr> 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<DecisionTreeFactor>(
message);
else
beliefAtKey =
boost::make_shared<DecisionTreeFactor>(
(*beliefAtKey)
* (*boost::dynamic_pointer_cast<DecisionTreeFactor>(
message)));
boost::dynamic_pointer_cast<DecisionTreeFactor>(message);
else
beliefAtKey = boost::make_shared<DecisionTreeFactor>(
(*beliefAtKey) *
(*boost::dynamic_pointer_cast<DecisionTreeFactor>(message)));
}
if (starGraphs_.at(key).unary)
beliefAtKey = boost::make_shared<DecisionTreeFactor>(
@ -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<DecisionTreeFactor>((*beliefAtKey) / sumFactor);
beliefAtKey =
boost::make_shared<DecisionTreeFactor>((*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<Key, DiscreteFactor::shared_ptr> 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<DecisionTreeFactor>(
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<DecisionTreeFactor>(
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<DecisionTreeFactor>(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<Key, DiscreteKey>& 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<DecisionTreeFactor>(
*prodOfUnaries
* (*boost::dynamic_pointer_cast<DecisionTreeFactor>(
graph.at(factorIndex))));
*prodOfUnaries *
(*boost::dynamic_pointer_cast<DecisionTreeFactor>(
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();
}
}
/* ************************************************************************* */

53
gtsam_unstable/discrete/tests/testScheduler.cpp
View File

@ -5,14 +5,13 @@
*/
//#define ENABLE_TIMING
#include <gtsam_unstable/discrete/Scheduler.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/timing.h>
#include <gtsam_unstable/discrete/Scheduler.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/map.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/optional.hpp>
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);
}
/* ************************************************************************* */

238
gtsam_unstable/discrete/tests/testSudoku.cpp
View File

@ -5,74 +5,69 @@
* @author Frank Dellaert
*/
#include <gtsam_unstable/discrete/CSP.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/discrete/CSP.h>
#include <boost/assign/std/map.hpp>
using boost::assign::insert;
#include <stdarg.h>
#include <iostream>
#include <sstream>
#include <stdarg.h>
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<size_t, size_t> IJ;
/// Mapping from base i,j coordinates to discrete keys:
using IJ = std::pair<size_t, size_t>;
std::map<IJ, DiscreteKey> 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);
}
/* ************************************************************************* */

Some files were not shown because too many files have changed in this diff Show More