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

Merge branch 'develop' into hybrid/elimination

release/4.3a0
Frank Dellaert 2022-12-31 14:43:25 -05:00
parent d0a56da726 3e86280381
commit 1e9cbebfb0
32 changed files with 262 additions and 244 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/discrete/Assignment.h
View File

@ -51,6 +51,13 @@ class Assignment : public std::map<L, size_t> {
public:
using std::map<L, size_t>::operator=;
// Define the implicit default constructor.
Assignment() = default;
// Construct from initializer list.
Assignment(std::initializer_list<std::pair<const L, size_t>> init)
: std::map<L, size_t>{init} {}
void print(const std::string& s = "Assignment: ",
const std::function<std::string(L)>& labelFormatter =
&DefaultFormatter) const {

42
gtsam/discrete/DiscreteValues.cpp
View File

@ -17,6 +17,7 @@
#include <gtsam/discrete/DiscreteValues.h>
#include <boost/range/combine.hpp>
#include <sstream>
using std::cout;
@ -26,6 +27,7 @@ using std::stringstream;
namespace gtsam {
/* ************************************************************************ */
void DiscreteValues::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << ": ";
@ -34,6 +36,44 @@ void DiscreteValues::print(const string& s,
cout << endl;
}
/* ************************************************************************ */
bool DiscreteValues::equals(const DiscreteValues& x, double tol) const {
if (this->size() != x.size()) return false;
for (const auto values : boost::combine(*this, x)) {
if (values.get<0>() != values.get<1>()) return false;
}
return true;
}
/* ************************************************************************ */
DiscreteValues& DiscreteValues::insert(const DiscreteValues& values) {
for (const auto& kv : values) {
if (count(kv.first)) {
throw std::out_of_range(
"Requested to insert a DiscreteValues into another DiscreteValues "
"that already contains one or more of its keys.");
} else {
this->emplace(kv);
}
}
return *this;
}
/* ************************************************************************ */
DiscreteValues& DiscreteValues::update(const DiscreteValues& values) {
for (const auto& kv : values) {
if (!count(kv.first)) {
throw std::out_of_range(
"Requested to update a DiscreteValues with another DiscreteValues "
"that contains keys not present in the first.");
} else {
(*this)[kv.first] = kv.second;
}
}
return *this;
}
/* ************************************************************************ */
string DiscreteValues::Translate(const Names& names, Key key, size_t index) {
if (names.empty()) {
stringstream ss;
@ -60,6 +100,7 @@ string DiscreteValues::markdown(const KeyFormatter& keyFormatter,
return ss.str();
}
/* ************************************************************************ */
string DiscreteValues::html(const KeyFormatter& keyFormatter,
const Names& names) const {
stringstream ss;
@ -84,6 +125,7 @@ string DiscreteValues::html(const KeyFormatter& keyFormatter,
return ss.str();
}
/* ************************************************************************ */
string markdown(const DiscreteValues& values, const KeyFormatter& keyFormatter,
const DiscreteValues::Names& names) {
return values.markdown(keyFormatter, names);

48
gtsam/discrete/DiscreteValues.h
View File

@ -27,21 +27,16 @@
namespace gtsam {
/** A map from keys to values
* TODO(dellaert): Do we need this? Should we just use gtsam::DiscreteValues?
* We just need another special DiscreteValue to represent labels,
* However, all other Lie's operators are undefined in this class.
* The good thing is we can have a Hybrid graph of discrete/continuous variables
* together..
* Another good thing is we don't need to have the special DiscreteKey which
* stores cardinality of a Discrete variable. It should be handled naturally in
* the new class DiscreteValue, as the variable's type (domain)
/**
* A map from keys to values
* @ingroup discrete
*/
class GTSAM_EXPORT DiscreteValues : public Assignment<Key> {
public:
using Base = Assignment<Key>; // base class
/// @name Standard Constructors
/// @{
using Assignment::Assignment; // all constructors
// Define the implicit default constructor.
@ -50,14 +45,49 @@ class GTSAM_EXPORT DiscreteValues : public Assignment<Key> {
// Construct from assignment.
explicit DiscreteValues(const Base& a) : Base(a) {}
// Construct from initializer list.
DiscreteValues(std::initializer_list<std::pair<const Key, size_t>> init)
: Assignment<Key>{init} {}
/// @}
/// @name Testable
/// @{
/// print required by Testable.
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// equals required by Testable for unit testing.
bool equals(const DiscreteValues& x, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
// insert in base class;
std::pair<iterator, bool> insert( const value_type& value ){
return Base::insert(value);
}
/** Insert all values from \c values. Throws an invalid_argument exception if
* any keys to be inserted are already used. */
DiscreteValues& insert(const DiscreteValues& values);
/** For all key/value pairs in \c values, replace values with corresponding
* keys in this object with those in \c values. Throws std::out_of_range if
* any keys in \c values are not present in this object. */
DiscreteValues& update(const DiscreteValues& values);
/**
* @brief Return a vector of DiscreteValues, one for each possible
* combination of values.
*/
static std::vector<DiscreteValues> CartesianProduct(
const DiscreteKeys& keys) {
return Base::CartesianProduct<DiscreteValues>(keys);
}
/// @}
/// @name Wrapper support
/// @{

3
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -25,10 +25,7 @@
#include <gtsam/discrete/DecisionTree-inl.h> // for convert only
#define DISABLE_TIMING
#include <boost/assign/std/map.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/tokenizer.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/timing.h>

3
gtsam/discrete/tests/testDecisionTree.cpp
View File

@ -27,9 +27,6 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
using namespace std;
using namespace gtsam;

3
gtsam/discrete/tests/testDecisionTreeFactor.cpp
View File

@ -23,9 +23,6 @@
#include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/discrete/Signature.h>
#include <boost/assign/std/map.hpp>
using namespace boost::assign;
using namespace std;
using namespace gtsam;

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

@ -26,11 +26,6 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/list_inserter.hpp>
#include <boost/assign/std/map.hpp>
using namespace boost::assign;
#include <iostream>
#include <string>
#include <vector>
@ -115,11 +110,11 @@ TEST(DiscreteBayesNet, Asia) {
EXPECT(assert_equal(expected2, *chordal->back()));
// now sample from it
DiscreteValues expectedSample;
DiscreteValues 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}};
SETDEBUG("DiscreteConditional::sample", false);
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);
auto actualSample = chordal2->sample();
EXPECT(assert_equal(expectedSample, actualSample));
}

3
gtsam/discrete/tests/testDiscreteBayesTree.cpp
View File

@ -21,9 +21,6 @@
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/inference/BayesNet.h>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <iostream>

3
gtsam/discrete/tests/testDiscreteConditional.cpp
View File

@ -17,10 +17,7 @@
* @date Feb 14, 2011
*/
#include <boost/assign/std/map.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/make_shared.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/discrete/DecisionTreeFactor.h>

3
gtsam/discrete/tests/testDiscreteFactor.cpp
View File

@ -21,9 +21,6 @@
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/discrete/DiscreteFactor.h>
#include <boost/assign/std/map.hpp>
using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;

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

@ -23,9 +23,6 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
using namespace boost::assign;
using namespace std;
using namespace gtsam;
@ -49,9 +46,7 @@ TEST_UNSAFE(DiscreteFactorGraph, debugScheduler) {
// Check MPE.
auto actualMPE = graph.optimize();
DiscreteValues mpe;
insert(mpe)(0, 2)(1, 1)(2, 0)(3, 0);
EXPECT(assert_equal(mpe, actualMPE));
EXPECT(assert_equal({{0, 2}, {1, 1}, {2, 0}, {3, 0}}, actualMPE));
}
/* ************************************************************************* */
@ -149,8 +144,7 @@ TEST(DiscreteFactorGraph, test) {
EXPECT(assert_equal(expectedBayesNet, *actual2));
// Test mpe
DiscreteValues mpe;
insert(mpe)(0, 0)(1, 0)(2, 0);
DiscreteValues mpe { {0, 0}, {1, 0}, {2, 0}};
auto actualMPE = graph.optimize();
EXPECT(assert_equal(mpe, actualMPE));
EXPECT_DOUBLES_EQUAL(9, graph(mpe), 1e-5); // regression
@ -182,8 +176,7 @@ TEST_UNSAFE(DiscreteFactorGraph, testMaxProduct) {
graph.add(C & B, "0.1 0.9 0.4 0.6");
// Created expected MPE
DiscreteValues mpe;
insert(mpe)(0, 0)(1, 1)(2, 1);
DiscreteValues mpe{{0, 0}, {1, 1}, {2, 1}};
// Do max-product with different orderings
for (Ordering::OrderingType orderingType :
@ -209,8 +202,7 @@ TEST(DiscreteFactorGraph, marginalIsNotMPE) {
bayesNet.add(A % "10/9");
// The expected MPE is A=1, B=1
DiscreteValues mpe;
insert(mpe)(0, 1)(1, 1);
DiscreteValues mpe { {0, 1}, {1, 1} };
// Which we verify using max-product:
DiscreteFactorGraph graph(bayesNet);
@ -240,8 +232,7 @@ TEST(DiscreteFactorGraph, testMPE_Darwiche09book_p244) {
graph.add(T1 & T2 & A, "1 0 0 1 0 1 1 0");
graph.add(A, "1 0"); // evidence, A = yes (first choice in Darwiche)
DiscreteValues mpe;
insert(mpe)(4, 0)(2, 1)(3, 1)(0, 1)(1, 1);
DiscreteValues mpe { {0, 1}, {1, 1}, {2, 1}, {3, 1}, {4, 0}};
EXPECT_DOUBLES_EQUAL(0.33858, graph(mpe), 1e-5); // regression
// You can check visually by printing product:
// graph.product().print("Darwiche-product");
@ -267,112 +258,6 @@ TEST(DiscreteFactorGraph, testMPE_Darwiche09book_p244) {
EXPECT_LONGS_EQUAL(2, bayesTree->size());
}
#ifdef OLD
/* ************************************************************************* */
/**
* Key type for discrete conditionals
* Includes name and cardinality
*/
class Key2 {
private:
std::string wff_;
size_t cardinality_;
public:
/** Constructor, defaults to binary */
Key2(const std::string& name, size_t cardinality = 2) :
wff_(name), cardinality_(cardinality) {
}
const std::string& name() const {
return wff_;
}
/** provide streaming */
friend std::ostream& operator <<(std::ostream &os, const Key2 &key);
};
struct Factor2 {
std::string wff_;
Factor2() :
wff_("@") {
}
Factor2(const std::string& s) :
wff_(s) {
}
Factor2(const Key2& key) :
wff_(key.name()) {
}
friend std::ostream& operator <<(std::ostream &os, const Factor2 &f);
friend Factor2 operator -(const Key2& key);
};
std::ostream& operator <<(std::ostream &os, const Factor2 &f) {
os << f.wff_;
return os;
}
/** negation */
Factor2 operator -(const Key2& key) {
return Factor2("-" + key.name());
}
/** OR */
Factor2 operator ||(const Factor2 &factor1, const Factor2 &factor2) {
return Factor2(std::string("(") + factor1.wff_ + " || " + factor2.wff_ + ")");
}
/** AND */
Factor2 operator &&(const Factor2 &factor1, const Factor2 &factor2) {
return Factor2(std::string("(") + factor1.wff_ + " && " + factor2.wff_ + ")");
}
/** implies */
Factor2 operator >>(const Factor2 &factor1, const Factor2 &factor2) {
return Factor2(std::string("(") + factor1.wff_ + " >> " + factor2.wff_ + ")");
}
struct Graph2: public std::list<Factor2> {
/** Add a factor graph*/
// void operator +=(const Graph2& graph) {
// for(const Factor2& f: graph)
// push_back(f);
// }
friend std::ostream& operator <<(std::ostream &os, const Graph2& graph);
};
/** Add a factor */
//Graph2 operator +=(Graph2& graph, const Factor2& factor) {
// graph.push_back(factor);
// return graph;
//}
std::ostream& operator <<(std::ostream &os, const Graph2& graph) {
for(const Factor2& f: graph)
os << f << endl;
return os;
}
/* ************************************************************************* */
TEST(DiscreteFactorGraph, Sugar)
{
Key2 M("Mythical"), I("Immortal"), A("Mammal"), H("Horned"), G("Magical");
// Test this desired construction
Graph2 unicorns;
unicorns += M >> -A;
unicorns += (-M) >> (-I && A);
unicorns += (I || A) >> H;
unicorns += H >> G;
// should be done by adapting boost::assign:
// unicorns += (-M) >> (-I && A), (I || A) >> H , H >> G;
cout << unicorns;
}
#endif
/* ************************************************************************* */
TEST(DiscreteFactorGraph, Dot) {
// Create Factor graph

7
gtsam/discrete/tests/testDiscreteLookupDAG.cpp
View File

@ -20,11 +20,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DiscreteLookupDAG.h>
#include <boost/assign/list_inserter.hpp>
#include <boost/assign/std/map.hpp>
using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */
TEST(DiscreteLookupDAG, argmax) {
@ -43,8 +39,7 @@ TEST(DiscreteLookupDAG, argmax) {
dag.add(1, DiscreteKeys{A}, adtA);
// The expected MPE is A=1, B=1
DiscreteValues mpe;
insert(mpe)(0, 1)(1, 1);
DiscreteValues mpe{{0, 1}, {1, 1}};
// check:
auto actualMPE = dag.argmax();

3
gtsam/discrete/tests/testDiscreteMarginals.cpp
View File

@ -19,9 +19,6 @@
#include <gtsam/discrete/DiscreteMarginals.h>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
using namespace std;

29
gtsam/discrete/tests/testDiscreteValues.cpp
View File

@ -21,18 +21,28 @@
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/discrete/Signature.h>
#include <boost/assign/std/map.hpp>
using namespace boost::assign;
using namespace std;
using namespace gtsam;
static const DiscreteValues kExample{{12, 1}, {5, 0}};
/* ************************************************************************* */
// Check insert
TEST(DiscreteValues, Insert) {
EXPECT(assert_equal({{12, 1}, {5, 0}, {13, 2}},
DiscreteValues(kExample).insert({{13, 2}})));
}
/* ************************************************************************* */
// Check update.
TEST(DiscreteValues, Update) {
EXPECT(assert_equal({{12, 2}, {5, 0}},
DiscreteValues(kExample).update({{12, 2}})));
}
/* ************************************************************************* */
// Check markdown representation with a value formatter.
TEST(DiscreteValues, markdownWithValueFormatter) {
DiscreteValues values;
values[12] = 1; // A
values[5] = 0; // B
string expected =
"|Variable|value|\n"
"|:-:|:-:|\n"
@ -40,16 +50,13 @@ TEST(DiscreteValues, markdownWithValueFormatter) {
"|A|One|\n";
auto keyFormatter = [](Key key) { return key == 12 ? "A" : "B"; };
DiscreteValues::Names names{{12, {"Zero", "One", "Two"}}, {5, {"-", "+"}}};
string actual = values.markdown(keyFormatter, names);
string actual = kExample.markdown(keyFormatter, names);
EXPECT(actual == expected);
}
/* ************************************************************************* */
// Check html representation with a value formatter.
TEST(DiscreteValues, htmlWithValueFormatter) {
DiscreteValues values;
values[12] = 1; // A
values[5] = 0; // B
string expected =
"<div>\n"
"<table class='DiscreteValues'>\n"
@ -64,7 +71,7 @@ TEST(DiscreteValues, htmlWithValueFormatter) {
"</div>";
auto keyFormatter = [](Key key) { return key == 12 ? "A" : "B"; };
DiscreteValues::Names names{{12, {"Zero", "One", "Two"}}, {5, {"-", "+"}}};
string actual = values.html(keyFormatter, names);
string actual = kExample.html(keyFormatter, names);
EXPECT(actual == expected);
}

10
gtsam/discrete/tests/testSignature.cpp
View File

@ -21,12 +21,10 @@
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/Signature.h>
#include <boost/assign/std/vector.hpp>
#include <vector>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
DiscreteKey X(0, 2), Y(1, 3), Z(2, 2);
@ -57,12 +55,8 @@ TEST(testSignature, simple_conditional) {
/* ************************************************************************* */
TEST(testSignature, simple_conditional_nonparser) {
Signature::Table table;
Signature::Row row1, row2, row3;
row1 += 1.0, 1.0;
row2 += 2.0, 3.0;
row3 += 1.0, 4.0;
table += row1, row2, row3;
Signature::Row row1{1, 1}, row2{2, 3}, row3{1, 4};
Signature::Table table{row1, row2, row3};
Signature sig(X | Y = table);
CHECK(sig.key() == X);

2
gtsam/hybrid/GaussianMixture.cpp
View File

@ -210,7 +210,7 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) {
DiscreteValues::CartesianProduct(set_diff);
for (const DiscreteValues &assignment : assignments) {
DiscreteValues augmented_values(values);
augmented_values.insert(assignment.begin(), assignment.end());
augmented_values.insert(assignment);
// If any one of the sub-branches are non-zero,
// we need this conditional.

2
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -112,7 +112,7 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
DiscreteValues::CartesianProduct(set_diff);
for (const DiscreteValues &assignment : assignments) {
DiscreteValues augmented_values(values);
augmented_values.insert(assignment.begin(), assignment.end());
augmented_values.insert(assignment);
// If any one of the sub-branches are non-zero,
// we need this probability.

53
gtsam/hybrid/HybridValues.h
View File

@ -104,6 +104,28 @@ class GTSAM_EXPORT HybridValues {
* @param j The index with which the value will be associated. */
void insert(Key j, const Vector& value) { continuous_.insert(j, value); }
/** Insert all continuous values from \c values. Throws an invalid_argument
* exception if any keys to be inserted are already used. */
HybridValues& insert(const VectorValues& values) {
continuous_.insert(values);
return *this;
}
/** Insert all discrete values from \c values. Throws an invalid_argument
* exception if any keys to be inserted are already used. */
HybridValues& insert(const DiscreteValues& values) {
discrete_.insert(values);
return *this;
}
/** Insert all values from \c values. Throws an invalid_argument exception if
* any keys to be inserted are already used. */
HybridValues& insert(const HybridValues& values) {
continuous_.insert(values.continuous());
discrete_.insert(values.discrete());
return *this;
}
// TODO(Shangjie)- insert_or_assign() , similar to Values.h
/**
@ -118,10 +140,33 @@ class GTSAM_EXPORT HybridValues {
*/
Vector& at(Key j) { return continuous_.at(j); };
/** For all key/value pairs in \c values, replace values with corresponding keys in this class
* with those in \c values. Throws std::out_of_range if any keys in \c values are not present
* in this class. */
void update(const VectorValues& values) { continuous_.update(values); }
/** For all key/value pairs in \c values, replace continuous values with
* corresponding keys in this object with those in \c values. Throws
* std::out_of_range if any keys in \c values are not present in this object.
*/
HybridValues& update(const VectorValues& values) {
continuous_.update(values);
return *this;
}
/** For all key/value pairs in \c values, replace discrete values with
* corresponding keys in this object with those in \c values. Throws
* std::out_of_range if any keys in \c values are not present in this object.
*/
HybridValues& update(const DiscreteValues& values) {
discrete_.update(values);
return *this;
}
/** For all key/value pairs in \c values, replace all values with
* corresponding keys in this object with those in \c values. Throws
* std::out_of_range if any keys in \c values are not present in this object.
*/
HybridValues& update(const HybridValues& values) {
continuous_.update(values.continuous());
discrete_.update(values.discrete());
return *this;
}
/// @}
/// @name Wrapper support

9
gtsam/hybrid/hybrid.i
View File

@ -15,9 +15,18 @@ class HybridValues {
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::HybridValues& other, double tol) const;
void insert(gtsam::Key j, int value);
void insert(gtsam::Key j, const gtsam::Vector& value);
void insert(const gtsam::VectorValues& values);
void insert(const gtsam::DiscreteValues& values);
void insert(const gtsam::HybridValues& values);
void update(const gtsam::VectorValues& values);
void update(const gtsam::DiscreteValues& values);
void update(const gtsam::HybridValues& values);
size_t& atDiscrete(gtsam::Key j);
gtsam::Vector& at(gtsam::Key j);
};

3
gtsam/hybrid/tests/testHybridBayesNet.cpp
View File

@ -237,8 +237,7 @@ TEST(HybridBayesNet, Error) {
EXPECT(assert_equal(expected_pruned_error, pruned_error_tree, 1e-9));
// Verify error computation and check for specific error value
DiscreteValues discrete_values;
boost::assign::insert(discrete_values)(M(0), 1)(M(1), 1);
DiscreteValues discrete_values {{M(0), 1}, {M(1), 1}};
double total_error = 0;
for (size_t idx = 0; idx < hybridBayesNet->size(); idx++) {

3
gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
View File

@ -40,7 +40,6 @@
#include <gtsam/linear/JacobianFactor.h>
#include <algorithm>
#include <boost/assign/std/map.hpp>
#include <cstddef>
#include <functional>
#include <iostream>
@ -49,8 +48,6 @@
#include "Switching.h"
using namespace boost::assign;
using namespace std;
using namespace gtsam;

35
gtsam/hybrid/tests/testHybridValues.cpp
View File

@ -32,22 +32,45 @@
using namespace std;
using namespace gtsam;
TEST(HybridValues, basics) {
static const HybridValues kExample{{{99, Vector2(2, 3)}}, {{100, 3}}};
/* ************************************************************************* */
TEST(HybridValues, Basics) {
HybridValues values;
values.insert(99, Vector2(2, 3));
values.insert(100, 3);
EXPECT(assert_equal(kExample, values));
EXPECT(assert_equal(values.at(99), Vector2(2, 3)));
EXPECT(assert_equal(values.atDiscrete(100), int(3)));
values.print();
HybridValues values2;
values2.insert(100, 3);
values2.insert(99, Vector2(2, 3));
EXPECT(assert_equal(values2, values));
EXPECT(assert_equal(kExample, values2));
}
values2.insert(98, Vector2(2, 3));
EXPECT(!assert_equal(values2, values));
/* ************************************************************************* */
// Check insert
TEST(HybridValues, Insert) {
HybridValues actual;
EXPECT(assert_equal({{}, {{100, 3}}}, //
actual.insert(DiscreteValues{{100, 3}})));
EXPECT(assert_equal(kExample, //
actual.insert(VectorValues{{99, Vector2(2, 3)}})));
HybridValues actual2;
EXPECT(assert_equal(kExample, actual2.insert(kExample)));
}
/* ************************************************************************* */
// Check update.
TEST(HybridValues, Update) {
HybridValues actual(kExample);
EXPECT(assert_equal({{{99, Vector2(2, 3)}}, {{100, 2}}},
actual.update(DiscreteValues{{100, 2}})));
EXPECT(assert_equal({{{99, Vector1(4)}}, {{100, 2}}},
actual.update(VectorValues{{99, Vector1(4)}})));
HybridValues actual2(kExample);
EXPECT(assert_equal(kExample, actual2.update(kExample)));
}
/* ************************************************************************* */

26
gtsam/linear/VectorValues.cpp
View File

@ -98,30 +98,34 @@ namespace gtsam {
}
/* ************************************************************************ */
void VectorValues::update(const VectorValues& values)
{
VectorValues& VectorValues::update(const VectorValues& values) {
iterator hint = begin();
for(const KeyValuePair& key_value: values)
{
// Use this trick to find the value using a hint, since we are inserting from another sorted map
for (const KeyValuePair& key_value : values) {
// Use this trick to find the value using a hint, since we are inserting
// from another sorted map
size_t oldSize = values_.size();
hint = values_.insert(hint, key_value);
if(values_.size() > oldSize) {
if (values_.size() > oldSize) {
values_.unsafe_erase(hint);
throw out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first.");
throw out_of_range(
"Requested to update a VectorValues with another VectorValues that "
"contains keys not present in the first.");
} else {
hint->second = key_value.second;
}
}
return *this;
}
/* ************************************************************************ */
void VectorValues::insert(const VectorValues& values)
{
VectorValues& VectorValues::insert(const VectorValues& values) {
size_t originalSize = size();
values_.insert(values.begin(), values.end());
if(size() != originalSize + values.size())
throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys.");
if (size() != originalSize + values.size())
throw invalid_argument(
"Requested to insert a VectorValues into another VectorValues that "
"already contains one or more of its keys.");
return *this;
}
/* ************************************************************************ */

15
gtsam/linear/VectorValues.h
View File

@ -88,11 +88,16 @@ namespace gtsam {
/// @name Standard Constructors
/// @{
/**
* Default constructor creates an empty VectorValues.
*/
/// Default constructor creates an empty VectorValues.
VectorValues() {}
/// Construct from initializer list.
VectorValues(std::initializer_list<std::pair<Key, Vector>> init) {
for (const auto& p : init) {
values_.insert(p); // Insert key-value pair into map
}
}
/** Merge two VectorValues into one, this is more efficient than inserting
* elements one by one. */
VectorValues(const VectorValues& first, const VectorValues& second);
@ -167,7 +172,7 @@ namespace gtsam {
/** For all key/value pairs in \c values, replace values with corresponding keys in this class
* with those in \c values. Throws std::out_of_range if any keys in \c values are not present
* in this class. */
void update(const VectorValues& values);
VectorValues& update(const VectorValues& values);
/** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c
* j is already used.
@ -198,7 +203,7 @@ namespace gtsam {
/** Insert all values from \c values. Throws an invalid_argument exception if any keys to be
* inserted are already used. */
void insert(const VectorValues& values);
VectorValues& insert(const VectorValues& values);
/** insert that mimics the STL map insert - if the value already exists, the map is not modified
* and an iterator to the existing value is returned, along with 'false'. If the value did not

1
gtsam/linear/linear.i
View File

@ -237,6 +237,7 @@ class VectorValues {
void insert(size_t j, Vector value);
Vector vector() const;
Vector at(size_t j) const;
void insert(const gtsam::VectorValues& values);
void update(const gtsam::VectorValues& values);
//Advanced Interface

16
gtsam/linear/tests/testVectorValues.cpp
View File

@ -73,6 +73,22 @@ TEST(VectorValues, basics)
CHECK_EXCEPTION(actual.dim(3), out_of_range);
}
/* ************************************************************************* */
static const VectorValues kExample = {{99, Vector2(2, 3)}};
// Check insert
TEST(VectorValues, Insert) {
VectorValues actual;
EXPECT(assert_equal(kExample, actual.insert(kExample)));
}
// Check update.
TEST(VectorValues, Update) {
VectorValues actual(kExample);
EXPECT(assert_equal(kExample, actual.update(kExample)));
}
/* ************************************************************************* */
TEST(VectorValues, combine)
{

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

@ -12,14 +12,11 @@
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/map.hpp>
#include <boost/optional.hpp>
#include <boost/format.hpp>
#include <algorithm>
using namespace boost::assign;
using namespace std;
using namespace gtsam;

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

@ -12,14 +12,11 @@
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/map.hpp>
#include <boost/optional.hpp>
#include <boost/format.hpp>
#include <algorithm>
using namespace boost::assign;
using namespace std;
using namespace gtsam;

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

@ -12,14 +12,11 @@
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/map.hpp>
#include <boost/optional.hpp>
#include <boost/format.hpp>
#include <algorithm>
using namespace boost::assign;
using namespace std;
using namespace gtsam;

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

@ -8,8 +8,6 @@
#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 <fstream>
@ -133,8 +131,7 @@ TEST(CSP, allInOne) {
// Solve
auto mpe = csp.optimize();
DiscreteValues expected;
insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1);
DiscreteValues expected {{ID.first, 1}, {UT.first, 0}, {AZ.first, 1}};
EXPECT(assert_equal(expected, mpe));
EXPECT_DOUBLES_EQUAL(1, csp(mpe), 1e-9);
}
@ -172,8 +169,8 @@ TEST(CSP, WesternUS) {
csp.addAllDiff(WY, CO);
csp.addAllDiff(CO, NM);
DiscreteValues mpe;
insert(mpe)(0, 2)(1, 3)(2, 2)(3, 1)(4, 1)(5, 3)(6, 3)(7, 2)(8, 0)(9, 1)(10, 0);
DiscreteValues mpe{{0, 2}, {1, 3}, {2, 2}, {3, 1}, {4, 1}, {5, 3},
{6, 3}, {7, 2}, {8, 0}, {9, 1}, {10, 0}};
// Create ordering according to example in ND-CSP.lyx
Ordering ordering;
@ -224,8 +221,7 @@ TEST(CSP, ArcConsistency) {
// Solve
auto mpe = csp.optimize();
DiscreteValues expected;
insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2);
DiscreteValues expected {{ID.first, 1}, {UT.first, 0}, {AZ.first, 2}};
EXPECT(assert_equal(expected, mpe));
EXPECT_DOUBLES_EQUAL(1, csp(mpe), 1e-9);

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

@ -10,11 +10,8 @@
#include <gtsam/base/timing.h>
#include <gtsam_unstable/discrete/Scheduler.h>
#include <boost/assign/std/map.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/optional.hpp>
using namespace boost::assign;
using namespace std;
using namespace gtsam;

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

@ -9,8 +9,6 @@
#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>
@ -128,11 +126,14 @@ TEST(Sudoku, small) {
// optimize and check
auto solution = csp.optimize();
DiscreteValues 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);
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);