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

30
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,29 +76,33 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
// Derivative for points in intrinsic coords (2 by 2)
if (H2) {
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 dt_dr = 1 / (1 + r2);
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_dxi = dtd_dt * dt_dr * dr_dxi;
const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
const double dtd_dr = dtd_dt * dt_dr;
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 c2 = dr_dxi * dr_dxi;
const double s2 = dr_dyi * dr_dyi;
const double cs = dr_dxi * 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;
*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);

10
gtsam/geometry/Quaternion.h
View File

@ -117,6 +117,7 @@ struct traits<QUATERNION_TYPE> {
omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
} else {
// Normal, away from zero case
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)
@ -124,6 +125,15 @@ struct traits<QUATERNION_TYPE> {
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>());

2
gtsam/geometry/Similarity3.cpp
View File

@ -40,7 +40,7 @@ static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
}
/// Form inner products x and y and calculate scale.
static const double calculateScale(const Point3Pairs &d_abPointPairs,
static double calculateScale(const Point3Pairs &d_abPointPairs,
const Rot3 &aRb) {
double x = 0, y = 0;
Point3 da, db;

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

44
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>
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.
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,

16
gtsam/linear/GaussianFactorGraph.h
View File

@ -21,12 +21,13 @@
#pragma once
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.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/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam {
@ -395,9 +396,14 @@ namespace gtsam {
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated */
VectorValues optimize(boost::none_t,
const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
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));

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

@ -665,22 +665,11 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone)
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);
* }
*/
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_;

9
gtsam/nonlinear/factorTesting.h
View File

@ -36,7 +36,6 @@ namespace gtsam {
*/
JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
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;

106
gtsam_unstable/discrete/AllDiff.cpp
View File

@ -5,74 +5,77 @@
* @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 {
/* ************************************************************************* */
void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const {
std::cout << s << "AllDiff on ";
for (Key dkey: keys_)
std::cout << formatter(dkey) << " ";
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_) {
/* ************************************************************************* */
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
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 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));
BinaryAllDiff binary12(discreteKey(i1), discreteKey(i2));
converted = converted * binary12.toDecisionTreeFactor();
}
return converted;
}
}
/* ************************************************************************* */
DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const {
/* ************************************************************************* */
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);
/* ************************************************************************* */
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.
// 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
Domain& Dj = domains[j];
if (Dj.checkAllDiff(keys_, domains)) return true;
boost::optional<Domain> maybeChanged = Dj.checkAllDiff(keys_, *domains);
if (maybeChanged) {
Dj = *maybeChanged;
return true;
}
// Check all other domains for singletons and erase corresponding values
// 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_)
for (Key k : keys_)
if (k != j) {
const Domain& Dk = domains[k];
const Domain& Dk = domains->at(k);
if (Dk.isSingleton()) { // check if singleton
size_t value = Dk.firstValue();
if (Dj.contains(value)) {
@ -82,30 +85,29 @@ namespace gtsam {
}
}
return changed;
}
}
/* ************************************************************************* */
Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const {
/* ************************************************************************* */
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_)
for (Key k : keys_)
if (values.find(k) == values.end()) {
newKeys.push_back(DiscreteKey(k,cardinalities_.at(k)));
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 {
/* ************************************************************************* */
Constraint::shared_ptr AllDiff::partiallyApply(
const Domains& domains) const {
DiscreteFactor::Values known;
for(Key k: keys_) {
const Domain& Dk = domains[k];
if (Dk.isSingleton())
known[k] = Dk.firstValue();
for (Key k : keys_) {
const Domain& Dk = domains.at(k);
if (Dk.isSingleton()) known[k] = Dk.firstValue();
}
return partiallyApply(known);
}
}
/* ************************************************************************* */
/* ************************************************************************* */
} // namespace gtsam

45
gtsam_unstable/discrete/AllDiff.h
View File

@ -7,44 +7,39 @@
#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_;
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));
return DiscreteKey(j, cardinalities_.at(j));
}
public:
/// Constructor
/// Construct from keys.
AllDiff(const DiscreteKeys& dkeys);
// print
void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
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))
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(),
return cardinalities_.size() == f.cardinalities_.size() &&
std::equal(cardinalities_.begin(), cardinalities_.end(),
f.cardinalities_.begin());
}
}
@ -59,19 +54,19 @@ namespace gtsam {
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.
* Ensure Arc-consistency by checking every possible value of domain j.
* @param j domain to be checked
* @param domains all other domains
* @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(size_t j, std::vector<Domain>& domains) const override;
bool ensureArcConsistency(Key j, Domains* 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;
};
Constraint::shared_ptr partiallyApply(
const Domains&) const override;
};
} // namespace gtsam

56
gtsam_unstable/discrete/BinaryAllDiff.h
View File

@ -7,33 +7,29 @@
#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.
* Returns 1 if values for two keys are different, 0 otherwise.
*/
class BinaryAllDiff: public Constraint {
class BinaryAllDiff : public Constraint {
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) {
}
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 = "",
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;
@ -41,28 +37,28 @@ namespace gtsam {
/// equals
bool equals(const DiscreteFactor& other, double tol) const override {
if(!dynamic_cast<const BinaryAllDiff*>(&other))
if (!dynamic_cast<const BinaryAllDiff*>(&other))
return false;
else {
const BinaryAllDiff& f(static_cast<const BinaryAllDiff&>(other));
return (cardinality0_==f.cardinality0_) && (cardinality1_==f.cardinality1_);
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]));
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_));
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);
for (size_t i2 = 0; i2 < cardinality1_; i2++) table.push_back(i1 != i2);
DecisionTreeFactor converted(keys, table);
return converted;
}
@ -74,14 +70,14 @@ namespace gtsam {
}
/*
* Ensure Arc-consistency
* Ensure Arc-consistency by checking every possible value of domain j.
* @param j domain to be checked
* @param domains all other domains
* @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(size_t j, std::vector<Domain>& domains) const override {
// throw std::runtime_error(
// "BinaryAllDiff::ensureArcConsistency not implemented");
bool ensureArcConsistency(Key j, Domains* domains) const override {
throw std::runtime_error(
"BinaryAllDiff::ensureArcConsistency not implemented");
return false;
}
@ -92,9 +88,9 @@ namespace gtsam {
/// Partially apply known values, domain version
Constraint::shared_ptr partiallyApply(
const std::vector<Domain>&) const override {
const Domains&) const override {
throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented");
}
};
};
} // namespace gtsam

148
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 {
/// Find the best total assignment - can be expensive
CSP::Values CSP::optimalAssignment() const {
DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential();
sharedValues mpe = chordal->optimize();
return mpe;
}
return chordal->optimize();
}
/// Find the best total assignment - can be expensive
CSP::sharedValues CSP::optimalAssignment(const Ordering& ordering) const {
/// Find the best total assignment - can be expensive
CSP::Values CSP::optimalAssignment(const Ordering& ordering) const {
DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(ordering);
sharedValues mpe = chordal->optimize();
return mpe;
}
return chordal->optimize();
}
void CSP::runArcConsistency(size_t cardinality, size_t nrIterations, bool print) const {
bool CSP::runArcConsistency(const VariableIndex& index,
Domains* domains) const {
bool changed = false;
// 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;
// If this domain is already a singleton, we do nothing.
if (domains->at(key).isSingleton()) continue;
// 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;
}
}
}
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);
// index.print();
size_t n = index.size();
// Initialize domains
std::vector < Domain > domains;
for (size_t j = 0; j < n; j++)
domains.push_back(Domain(DiscreteKey(j,cardinality)));
// 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];
Domains domains;
for (auto entry : index) {
const Key key = entry.first;
domains.emplace(key, DiscreteKey(key, cardinality));
}
} // 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
// 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;
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();
}
#endif
}
} // gtsam
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

86
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
*/
class GTSAM_UNSTABLE_EXPORT CSP: public DiscreteFactorGraph {
class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph {
public:
/** A map from keys to values */
typedef KeyVector Indices;
typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> sharedValues;
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);
emplace_shared<SingleValue>(dkey, value);
}
/// 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);
emplace_shared<BinaryAllDiff>(key1, key2);
}
/// Add a general AllDiff constraint
void addAllDiff(const DiscreteKeys& dkeys) {
boost::shared_ptr<AllDiff> factor(new AllDiff(dkeys));
push_back(factor);
}
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;
// }
// /** 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.
Values optimalAssignment() const;
/// Find the best total assignment - can be expensive
sharedValues optimalAssignment(const Ordering& ordering) 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 nrIterations = 10) 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 Indices
* TODO: should get cardinality from DiscreteKeys
*/
void runArcConsistency(size_t cardinality, size_t nrIterations = 10,
bool print = false) const;
}; // CSP
Domains runArcConsistency(size_t cardinality,
size_t maxIterations = 10) const;
} // gtsam
/// Run arc consistency for all variables, return true if any domain changed.
bool runArcConsistency(const VariableIndex& index, Domains* domains) const;
/*
* Create a new CSP, applying the given Domain constraints.
*/
CSP partiallyApply(const Domains& domains) const;
}; // CSP
} // namespace gtsam

59
gtsam_unstable/discrete/Constraint.h
View File

@ -17,49 +17,42 @@
#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 Constraint : public DiscreteFactor {
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 n-way factor
Constraint(const KeyVector& js) :
DiscreteFactor(js) {
}
/// Construct binary constraint factor.
Constraint(Key j1, Key j2)
: DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) {}
/// 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 n-way constraint factor.
Constraint(const KeyVector& js) : DiscreteFactor(js) {}
/// construct from container
template<class KeyIterator>
Constraint(KeyIterator beginKey, KeyIterator endKey) :
DiscreteFactor(beginKey, endKey) {
}
template <class KeyIterator>
Constraint(KeyIterator beginKey, KeyIterator endKey)
: DiscreteFactor(beginKey, endKey) {}
public:
/// @name Standard Constructors
/// @{
@ -74,20 +67,20 @@ namespace gtsam {
/// @{
/*
* Ensure Arc-consistency
* Ensure Arc-consistency by checking every possible value of domain j.
* @param j domain to be checked
* @param domains all other domains
* @param (in/out) domains all domains, but only domains->at(j) will be checked.
* @return true if domains->at(j) was changed, false otherwise.
*/
virtual bool ensureArcConsistency(size_t j, std::vector<Domain>& domains) const = 0;
virtual bool ensureArcConsistency(Key j, Domains* 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;
virtual shared_ptr partiallyApply(const Domains&) const = 0;
/// @}
};
};
// DiscreteFactor
}// namespace gtsam
} // namespace gtsam

122
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;
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;
}
/* ************************************************************************* */
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;
}
/* ************************************************************************* */
double Domain::operator()(const Values& values) const {
return contains(values.at(keys_[0]));
}
/* ************************************************************************* */
string Domain::base1Str() const {
stringstream ss;
for (size_t v : values_) ss << v + 1;
return ss.str();
}
/* ************************************************************************* */
DecisionTreeFactor Domain::toDecisionTreeFactor() const {
/* ************************************************************************* */
double Domain::operator()(const Values& values) const {
return contains(values.at(key()));
}
/* ************************************************************************* */
DecisionTreeFactor Domain::toDecisionTreeFactor() const {
DiscreteKeys keys;
keys += DiscreteKey(keys_[0],cardinality_);
keys += DiscreteKey(key(), cardinality_);
vector<double> table;
for (size_t i1 = 0; i1 < cardinality_; ++i1)
table.push_back(contains(i1));
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 {
/* ************************************************************************* */
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_)
/* ************************************************************************* */
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;
}
}
/* ************************************************************************* */
bool Domain::checkAllDiff(const KeyVector keys, vector<Domain>& domains) {
Key j = keys_[0];
/* ************************************************************************* */
boost::optional<Domain> Domain::checkAllDiff(const KeyVector keys,
const Domains& domains) const {
Key j = key();
// for all values in this domain
for(size_t value: values_) {
for (const size_t value : values_) {
// for all connected domains
for(Key k: keys)
for (const 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
if (k != j && domains.at(k).contains(value)) goto found;
// Otherwise: return a singleton:
return Domain(this->discreteKey(), value);
found:;
}
return false; // we did not change it
}
return boost::none; // 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 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 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);
}
/* ************************************************************************* */
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

94
gtsam_unstable/discrete/Domain.h
View File

@ -7,81 +7,73 @@
#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 {
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);
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) {
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_) {
}
/// The one key
Key key() const { return keys_[0]; }
/// insert a value, non const :-(
void insert(size_t value) {
values_.insert(value);
}
// The associated discrete key
DiscreteKey discreteKey() const { return DiscreteKey(key(), cardinality_); }
/// erase a value, non const :-(
void erase(size_t value) {
values_.erase(value);
}
/// Insert a value, non const :-(
void insert(size_t value) { values_.insert(value); }
size_t nrValues() const {
return values_.size();
}
/// Erase a value, non const :-(
void erase(size_t value) { values_.erase(value); }
bool isSingleton() const {
return nrValues() == 1;
}
size_t nrValues() const { return values_.size(); }
size_t firstValue() const {
return *values_.begin();
}
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;
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))
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 (cardinality_ == f.cardinality_) && (values_ == f.values_);
}
}
bool contains(size_t value) const {
return values_.count(value)>0;
}
// 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;
@ -93,25 +85,29 @@ namespace gtsam {
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
/*
* Ensure Arc-consistency
* Ensure Arc-consistency by checking every possible value of domain j.
* @param j domain to be checked
* @param domains all other domains
* @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(size_t j, std::vector<Domain>& domains) const override;
bool ensureArcConsistency(Key j, Domains* 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
* 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
*/
bool checkAllDiff(const KeyVector keys, std::vector<Domain>& domains);
boost::optional<Domain> checkAllDiff(const KeyVector keys,
const Domains& domains) const;
/// 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;
};
Constraint::shared_ptr partiallyApply(const Domains& domains) const override;
};
} // namespace gtsam

213
gtsam_unstable/discrete/Scheduler.cpp
View File

@ -5,24 +5,22 @@
* @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)
{
Scheduler::Scheduler(size_t maxNrStudents, const string& filename)
: maxNrStudents_(maxNrStudents) {
typedef boost::tokenizer<boost::escaped_list_separator<char> > Tokenizer;
// open file
@ -38,8 +36,7 @@ namespace gtsam {
if (getline(is, line, '\r')) {
Tokenizer tok(line);
Tokenizer::iterator it = tok.begin();
for (++it; it != tok.end(); ++it)
addFaculty(*it);
for (++it; it != tok.end(); ++it) addFaculty(*it);
}
// for all remaining lines
@ -50,17 +47,16 @@ namespace gtsam {
Tokenizer::iterator it = tok.begin();
addSlot(*it++); // add slot
// add availability
for (; it != tok.end(); ++it)
available_ += (it->empty()) ? "0 " : "1 ";
for (; it != tok.end(); ++it) available_ += (it->empty()) ? "0 " : "1 ";
available_ += '\n';
}
} // constructor
} // 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_);
/** 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));
@ -69,71 +65,73 @@ namespace gtsam {
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();
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());
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 {
/** 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());
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());
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());
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) {
/** 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());
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);
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);
assert(facultyInArea_[areaName].size() == areaKey.second);
CSP::add(areaKey, facultyInArea_[areaName]);
if (debug) cout << "Advisor constraint " << areaName << endl;
assert(s.advisor_.size()==areaKey.second);
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_;
size_t dummyIndex = maxNrStudents_ * 3 + maxNrStudents_;
DiscreteKey dummy(dummyIndex, nrTimeSlots());
Potentials::ADT p(dummy & areaKey, available_); // available_ is Doodle string
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);
@ -145,25 +143,22 @@ namespace gtsam {
// 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) {
/** 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);
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 (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_);
for (const Student& s : students_) dkeys.push_back(s.key_);
addAllDiff(dkeys);
} else {
if (debug) cout << "Mutex for Students" << endl;
@ -175,128 +170,120 @@ namespace gtsam {
}
}
}
} // buildGraph
} // buildGraph
/** print */
void Scheduler::print(const string& s, const KeyFormatter& formatter) const {
/** print */
void Scheduler::print(const string& s, const KeyFormatter& formatter) const {
cout << s << " Faculty:" << endl;
for(const string& name: facultyName_)
cout << name << '\n';
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;
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_)
{
for (const FacultyInArea::value_type& it : facultyInArea_) {
cout << setw(12) << it.first << ": ";
for(double v: it.second)
cout << v << " ";
for (double v : it.second) cout << v << " ";
cout << '\n';
}
cout << endl;
cout << s << " Students:\n";
for (const Student& student: students_)
student.print();
for (const Student& student : students_) student.print();
cout << endl;
CSP::print(s + " Factor graph");
cout << endl;
} // print
} // print
/** Print readable form of assignment */
void Scheduler::printAssignment(sharedValues assignment) const {
/** 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);
Key j = 3 * maxNrStudents_ + s;
size_t slot = assignment.at(j);
cout << studentName(s) << " slot: " << slotName_[slot] << endl;
Key base = 3*s;
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]
size_t faculty = assignment.at(base + area);
cout << setw(12) << studentArea(s, area) << ": " << facultyName_[faculty]
<< endl;
}
cout << endl;
}
}
}
/** Special print for single-student case */
void Scheduler::printSpecial(sharedValues assignment) const {
Values::const_iterator it = assignment->begin();
/** 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 << setw(12) << studentArea(0, area) << ": " << facultyName_[f] << endl;
}
cout << endl;
}
}
/** Accumulate faculty stats */
void Scheduler::accumulateStats(sharedValues assignment, vector<
size_t>& stats) const {
/** 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;
Key base = 3 * s;
for (size_t area = 0; area < 3; area++) {
size_t f = assignment->at(base+area);
assert(f<stats.size());
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 {
/** 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);
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::sharedValues Scheduler::optimalAssignment() const {
/** 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();
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();
Values 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;
/** 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::sharedValues Scheduler::bestAssignment(
sharedValues bestSchedule) const {
sharedValues 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;
}
} // gtsam
}
} // namespace gtsam

65
gtsam_unstable/discrete/Scheduler.h
View File

@ -11,17 +11,15 @@
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 {
class GTSAM_UNSTABLE_EXPORT Scheduler : public CSP {
private:
/** Internal data structure for students */
struct Student {
std::string name_;
@ -29,15 +27,14 @@ namespace gtsam {
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) {
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] << " ";
for (size_t area = 0; area < 3; area++) cout << areaName_[area] << " ";
cout << std::endl;
}
};
@ -63,7 +60,6 @@ namespace gtsam {
std::vector<double> slotsAvailable_;
public:
/**
* Constructor
* We need to know the number of students in advance for ordering keys.
@ -79,26 +75,16 @@ namespace gtsam {
facultyName_.push_back(facultyName);
}
size_t nrFaculty() const {
return facultyName_.size();
}
size_t nrFaculty() const { return facultyName_.size(); }
/** boolean std::string of nrTimeSlots * nrFaculty */
void setAvailability(const std::string& available) {
available_ = available;
}
void setAvailability(const std::string& available) { available_ = available; }
void addSlot(const std::string& slotName) {
slotName_.push_back(slotName);
}
void addSlot(const std::string& slotName) { slotName_.push_back(slotName); }
size_t nrTimeSlots() const {
return slotName_.size();
}
size_t nrTimeSlots() const { return slotName_.size(); }
const std::string& slotName(size_t s) const {
return slotName_[s];
}
const std::string& slotName(size_t s) const { return slotName_[s]; }
/** slots available, boolean */
void setSlotsAvailable(const std::vector<double>& slotsAvailable) {
@ -107,7 +93,8 @@ namespace gtsam {
void addArea(const std::string& facultyName, const std::string& areaName) {
areaName_.push_back(areaName);
std::vector<double>& table = facultyInArea_[areaName]; // will create if needed
std::vector<double>& table =
facultyInArea_[areaName]; // will create if needed
if (table.empty()) table.resize(nrFaculty(), 0);
table[facultyIndex_[facultyName]] = 1;
}
@ -119,7 +106,8 @@ namespace gtsam {
Scheduler(size_t maxNrStudents, const std::string& filename);
/** 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;
const DiscreteKey& key(size_t s,
boost::optional<size_t> area = boost::none) const;
/** addStudent has to be called after adding slots and faculty */
void addStudent(const std::string& studentName, const std::string& area1,
@ -127,16 +115,15 @@ namespace gtsam {
const std::string& advisor);
/// current number of students
size_t nrStudents() const {
return students_.size();
}
size_t nrStudents() const { return students_.size(); }
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);
void addStudentSpecificConstraints(
size_t i, boost::optional<size_t> slot = boost::none);
/** Main routine that builds factor graph */
void buildGraph(size_t mutexBound = 7);
@ -147,29 +134,27 @@ namespace gtsam {
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/** Print readable form of assignment */
void printAssignment(sharedValues assignment) const;
void printAssignment(const Values& assignment) const;
/** Special print for single-student case */
void printSpecial(sharedValues assignment) const;
void printSpecial(const Values& assignment) const;
/** Accumulate faculty stats */
void accumulateStats(sharedValues assignment,
void accumulateStats(const Values& 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;
Values optimalAssignment() const;
/** find the assignment of students to slots with most possible committees */
sharedValues bestSchedule() const;
Values bestSchedule() const;
/** find the corresponding most desirable committee assignment */
sharedValues bestAssignment(sharedValues bestSchedule) const;
}; // Scheduler
} // gtsam
Values bestAssignment(const Values& bestSchedule) const;
}; // Scheduler
} // namespace gtsam

88
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;
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;
}
/* ************************************************************************* */
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_);
}
/* ************************************************************************* */
double SingleValue::operator()(const Values& values) const {
return (double)(values.at(keys_[0]) == value_);
}
/* ************************************************************************* */
DecisionTreeFactor SingleValue::toDecisionTreeFactor() const {
/* ************************************************************************* */
DecisionTreeFactor SingleValue::toDecisionTreeFactor() const {
DiscreteKeys keys;
keys += DiscreteKey(keys_[0],cardinality_);
keys += DiscreteKey(keys_[0], cardinality_);
vector<double> table;
for (size_t i1 = 0; i1 < cardinality_; i1++)
table.push_back(i1 == value_);
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 {
/* ************************************************************************* */
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];
/* ************************************************************************* */
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_);
D = Domain(discreteKey(), value_);
return true;
}
}
/* ************************************************************************* */
Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const {
/* ************************************************************************* */
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_);
}
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_);
}
/* ************************************************************************* */
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

53
gtsam_unstable/discrete/SingleValue.h
View File

@ -7,51 +7,45 @@
#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 {
/// Number of values
size_t cardinality_;
/// allowed value
size_t value_;
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_);
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) {
}
/// Construct from key, cardinality, and given value.
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) {
}
/// 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;
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))
if (!dynamic_cast<const SingleValue*>(&other))
return false;
else {
const SingleValue& f(static_cast<const SingleValue&>(other));
return (cardinality_==f.cardinality_) && (value_==f.value_);
return (cardinality_ == f.cardinality_) && (value_ == f.value_);
}
}
@ -65,18 +59,19 @@ namespace gtsam {
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
/*
* Ensure Arc-consistency
* Ensure Arc-consistency: just sets domain[j] to {value_}.
* @param j domain to be checked
* @param domains all other domains
* @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(size_t j, std::vector<Domain>& domains) const override;
bool ensureArcConsistency(Key j, Domains* 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;
};
const Domains& domains) const override;
};
} // 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);

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

113
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 {
@ -37,40 +39,40 @@ class LoopyBelief {
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 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;
}
if (unary)
unary->print("Unary: ");
if (unary) unary->print("Unary: ");
}
};
typedef std::map<Key, StarGraph> StarGraphs;
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) {
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,8 +192,8 @@ private:
graph.at(factorIndex));
else
prodOfUnaries = boost::make_shared<DecisionTreeFactor>(
*prodOfUnaries
* (*boost::dynamic_pointer_cast<DecisionTreeFactor>(
*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();
}
}
/* ************************************************************************* */

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