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

Merge pull request #1380 from borglab/feature/uniform_error

release/4.3a0
Frank Dellaert 2023-01-12 07:48:01 -08:00 committed by GitHub
parent e9485d1d76 f4c3131ee2
commit a34c463e2b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
59 changed files with 829 additions and 422 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -18,6 +18,7 @@
*/
#include <gtsam/base/FastSet.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteConditional.h>
@ -56,6 +57,16 @@ namespace gtsam {
}
}
/* ************************************************************************ */
double DecisionTreeFactor::error(const DiscreteValues& values) const {
return -std::log(evaluate(values));
}
/* ************************************************************************ */
double DecisionTreeFactor::error(const HybridValues& values) const {
return error(values.discrete());
}
/* ************************************************************************ */
double DecisionTreeFactor::safe_div(const double& a, const double& b) {
// The use for safe_div is when we divide the product factor by the sum

22
gtsam/discrete/DecisionTreeFactor.h
View File

@ -34,6 +34,7 @@
namespace gtsam {
class DiscreteConditional;
class HybridValues;
/**
* A discrete probabilistic factor.
@ -97,11 +98,20 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/// Value is just look up in AlgebraicDecisonTree
/// Calculate probability for given values `x`,
/// is just look up in AlgebraicDecisionTree.
double evaluate(const DiscreteValues& values) const {
return ADT::operator()(values);
}
/// Evaluate probability density, sugar.
double operator()(const DiscreteValues& values) const override {
return ADT::operator()(values);
}
/// Calculate error for DiscreteValues `x`, is -log(probability).
double error(const DiscreteValues& values) const;
/// multiply two factors
DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
return apply(f, ADT::Ring::mul);
@ -230,6 +240,16 @@ namespace gtsam {
std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const Names& names = {}) const override;
/// @}
/// @name HybridValues methods.
/// @{
/**
* Calculate error for HybridValues `x`, is -log(probability)
* Simply dispatches to DiscreteValues version.
*/
double error(const HybridValues& values) const override;
/// @}
private:

9
gtsam/discrete/DiscreteBayesNet.cpp
View File

@ -33,6 +33,15 @@ bool DiscreteBayesNet::equals(const This& bn, double tol) const {
return Base::equals(bn, tol);
}
/* ************************************************************************* */
double DiscreteBayesNet::logProbability(const DiscreteValues& values) const {
// evaluate all conditionals and add
double result = 0.0;
for (const DiscreteConditional::shared_ptr& conditional : *this)
result += conditional->logProbability(values);
return result;
}
/* ************************************************************************* */
double DiscreteBayesNet::evaluate(const DiscreteValues& values) const {
// evaluate all conditionals and multiply

11
gtsam/discrete/DiscreteBayesNet.h
View File

@ -103,6 +103,9 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet<DiscreteConditional> {
return evaluate(values);
}
//** log(evaluate(values)) for given DiscreteValues */
double logProbability(const DiscreteValues & values) const;
/**
* @brief do ancestral sampling
*
@ -137,6 +140,14 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet<DiscreteConditional> {
const DiscreteFactor::Names& names = {}) const;
/// @}
/// @name HybridValues methods.
/// @{
using Base::error; // Expose error(const HybridValues&) method..
using Base::evaluate; // Expose evaluate(const HybridValues&) method..
using Base::logProbability; // Expose logProbability(const HybridValues&)
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// @name Deprecated functionality

22
gtsam/discrete/DiscreteConditional.h
View File

@ -18,9 +18,9 @@
#pragma once
#include <gtsam/inference/Conditional-inst.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/Signature.h>
#include <gtsam/inference/Conditional.h>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
@ -147,6 +147,11 @@ class GTSAM_EXPORT DiscreteConditional
/// @name Standard Interface
/// @{
/// Log-probability is just -error(x).
double logProbability(const DiscreteValues& x) const {
return -error(x);
}
/// print index signature only
void printSignature(
const std::string& s = "Discrete Conditional: ",
@ -225,6 +230,21 @@ class GTSAM_EXPORT DiscreteConditional
std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const Names& names = {}) const override;
/// @}
/// @name HybridValues methods.
/// @{
/**
* Calculate log-probability log(evaluate(x)) for HybridValues `x`.
* This is actually just -error(x).
*/
double logProbability(const HybridValues& x) const override {
return -error(x);
}
using DecisionTreeFactor::evaluate;
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42

11
gtsam/discrete/DiscreteFactor.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/base/Vector.h>
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <cmath>
#include <sstream>
@ -27,6 +28,16 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
double DiscreteFactor::error(const DiscreteValues& values) const {
return -std::log((*this)(values));
}
/* ************************************************************************* */
double DiscreteFactor::error(const HybridValues& c) const {
return this->error(c.discrete());
}
/* ************************************************************************* */
std::vector<double> expNormalize(const std::vector<double>& logProbs) {
double maxLogProb = -std::numeric_limits<double>::infinity();

10
gtsam/discrete/DiscreteFactor.h
View File

@ -27,6 +27,7 @@ namespace gtsam {
class DecisionTreeFactor;
class DiscreteConditional;
class HybridValues;
/**
* Base class for discrete probabilistic factors
@ -83,6 +84,15 @@ public:
/// Find value for given assignment of values to variables
virtual double operator()(const DiscreteValues&) const = 0;
/// Error is just -log(value)
double error(const DiscreteValues& values) const;
/**
* The Factor::error simply extracts the \class DiscreteValues from the
* \class HybridValues and calculates the error.
*/
double error(const HybridValues& c) const override;
/// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor
virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0;

6
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -222,6 +222,12 @@ class GTSAM_EXPORT DiscreteFactorGraph
std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const DiscreteFactor::Names& names = {}) const;
/// @}
/// @name HybridValues methods.
/// @{
using Base::error; // Expose error(const HybridValues&) method..
/// @}
}; // \ DiscreteFactorGraph

8
gtsam/discrete/discrete.i
View File

@ -95,6 +95,9 @@ virtual class DiscreteConditional : gtsam::DecisionTreeFactor {
DiscreteConditional(const gtsam::DecisionTreeFactor& joint,
const gtsam::DecisionTreeFactor& marginal,
const gtsam::Ordering& orderedKeys);
double logProbability(const gtsam::DiscreteValues& values) const;
double evaluate(const gtsam::DiscreteValues& values) const;
double operator()(const gtsam::DiscreteValues& values) const;
gtsam::DiscreteConditional operator*(
const gtsam::DiscreteConditional& other) const;
gtsam::DiscreteConditional marginal(gtsam::Key key) const;
@ -157,7 +160,12 @@ class DiscreteBayesNet {
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::DiscreteBayesNet& other, double tol = 1e-9) const;
// Standard interface.
double logProbability(const gtsam::DiscreteValues& values) const;
double evaluate(const gtsam::DiscreteValues& values) const;
double operator()(const gtsam::DiscreteValues& values) const;
gtsam::DiscreteValues sample() const;
gtsam::DiscreteValues sample(gtsam::DiscreteValues given) const;

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

@ -48,6 +48,9 @@ TEST( DecisionTreeFactor, constructors)
EXPECT_DOUBLES_EQUAL(8, f1(values), 1e-9);
EXPECT_DOUBLES_EQUAL(7, f2(values), 1e-9);
EXPECT_DOUBLES_EQUAL(75, f3(values), 1e-9);
// Assert that error = -log(value)
EXPECT_DOUBLES_EQUAL(-log(f1(values)), f1.error(values), 1e-9);
}
/* ************************************************************************* */

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

@ -25,7 +25,6 @@
#include <CppUnitLite/TestHarness.h>
#include <iostream>
#include <string>
#include <vector>
@ -101,6 +100,11 @@ TEST(DiscreteBayesNet, Asia) {
DiscreteConditional expected2(Bronchitis % "11/9");
EXPECT(assert_equal(expected2, *chordal->back()));
// Check evaluate and logProbability
auto result = fg.optimize();
EXPECT_DOUBLES_EQUAL(asia.logProbability(result),
std::log(asia.evaluate(result)), 1e-9);
// add evidence, we were in Asia and we have dyspnea
fg.add(Asia, "0 1");
fg.add(Dyspnea, "0 1");

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

@ -88,6 +88,29 @@ TEST(DiscreteConditional, constructors3) {
EXPECT(assert_equal(expected, static_cast<DecisionTreeFactor>(actual)));
}
/* ****************************************************************************/
// Test evaluate for a discrete Prior P(Asia).
TEST(DiscreteConditional, PriorProbability) {
constexpr Key asiaKey = 0;
const DiscreteKey Asia(asiaKey, 2);
DiscreteConditional dc(Asia, "4/6");
DiscreteValues values{{asiaKey, 0}};
EXPECT_DOUBLES_EQUAL(0.4, dc.evaluate(values), 1e-9);
}
/* ************************************************************************* */
// Check that error, logProbability, evaluate all work as expected.
TEST(DiscreteConditional, probability) {
DiscreteKey C(2, 2), D(4, 2), E(3, 2);
DiscreteConditional C_given_DE((C | D, E) = "4/1 1/1 1/1 1/4");
DiscreteValues given {{C.first, 1}, {D.first, 0}, {E.first, 0}};
EXPECT_DOUBLES_EQUAL(0.2, C_given_DE.evaluate(given), 1e-9);
EXPECT_DOUBLES_EQUAL(0.2, C_given_DE(given), 1e-9);
EXPECT_DOUBLES_EQUAL(log(0.2), C_given_DE.logProbability(given), 1e-9);
EXPECT_DOUBLES_EQUAL(-log(0.2), C_given_DE.error(given), 1e-9);
}
/* ************************************************************************* */
// Check calculation of joint P(A,B)
TEST(DiscreteConditional, Multiply) {

13
gtsam/hybrid/GaussianMixture.cpp
View File

@ -271,15 +271,16 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) {
}
/* *******************************************************************************/
AlgebraicDecisionTree<Key> GaussianMixture::error(
AlgebraicDecisionTree<Key> GaussianMixture::logProbability(
const VectorValues &continuousValues) const {
// functor to calculate to double error value from GaussianConditional.
// functor to calculate to double logProbability value from
// GaussianConditional.
auto errorFunc =
[continuousValues](const GaussianConditional::shared_ptr &conditional) {
if (conditional) {
return conditional->error(continuousValues);
return conditional->logProbability(continuousValues);
} else {
// Return arbitrarily large error if conditional is null
// Return arbitrarily large logProbability if conditional is null
// Conditional is null if it is pruned out.
return 1e50;
}
@ -289,10 +290,10 @@ AlgebraicDecisionTree<Key> GaussianMixture::error(
}
/* *******************************************************************************/
double GaussianMixture::error(const HybridValues &values) const {
double GaussianMixture::logProbability(const HybridValues &values) const {
// Directly index to get the conditional, no need to build the whole tree.
auto conditional = conditionals_(values.discrete());
return conditional->error(values.continuous());
return conditional->logProbability(values.continuous());
}
} // namespace gtsam

16
gtsam/hybrid/GaussianMixture.h
View File

@ -164,22 +164,23 @@ class GTSAM_EXPORT GaussianMixture
const Conditionals &conditionals() const;
/**
* @brief Compute error of the GaussianMixture as a tree.
* @brief Compute logProbability of the GaussianMixture as a tree.
*
* @param continuousValues The continuous VectorValues.
* @return AlgebraicDecisionTree<Key> A decision tree with the same keys
* as the conditionals, and leaf values as the error.
* as the conditionals, and leaf values as the logProbability.
*/
AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
AlgebraicDecisionTree<Key> logProbability(
const VectorValues &continuousValues) const;
/**
* @brief Compute the error of this Gaussian Mixture given the continuous
* values and a discrete assignment.
* @brief Compute the logProbability of this Gaussian Mixture given the
* continuous values and a discrete assignment.
*
* @param values Continuous values and discrete assignment.
* @return double
*/
double error(const HybridValues &values) const override;
double logProbability(const HybridValues &values) const override;
// /// Calculate probability density for given values `x`.
// double evaluate(const HybridValues &values) const;
@ -188,9 +189,6 @@ class GTSAM_EXPORT GaussianMixture
// double operator()(const HybridValues &values) const { return
// evaluate(values); }
// /// Calculate log-density for given values `x`.
// double logDensity(const HybridValues &values) const;
/**
* @brief Prune the decision tree of Gaussian factors as per the discrete
* `decisionTree`.

75
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -255,33 +255,6 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const {
return gbn.optimize();
}
/* ************************************************************************* */
double HybridBayesNet::evaluate(const HybridValues &values) const {
const DiscreteValues &discreteValues = values.discrete();
const VectorValues &continuousValues = values.continuous();
double logDensity = 0.0, probability = 1.0;
// Iterate over each conditional.
for (auto &&conditional : *this) {
// TODO: should be delegated to derived classes.
if (auto gm = conditional->asMixture()) {
const auto component = (*gm)(discreteValues);
logDensity += component->logDensity(continuousValues);
} else if (auto gc = conditional->asGaussian()) {
// If continuous only, evaluate the probability and multiply.
logDensity += gc->logDensity(continuousValues);
} else if (auto dc = conditional->asDiscrete()) {
// Conditional is discrete-only, so return its probability.
probability *= dc->operator()(discreteValues);
}
}
return probability * exp(logDensity);
}
/* ************************************************************************* */
HybridValues HybridBayesNet::sample(const HybridValues &given,
std::mt19937_64 *rng) const {
@ -318,45 +291,45 @@ HybridValues HybridBayesNet::sample() const {
}
/* ************************************************************************* */
double HybridBayesNet::error(const HybridValues &values) const {
GaussianBayesNet gbn = choose(values.discrete());
return gbn.error(values.continuous());
}
/* ************************************************************************* */
AlgebraicDecisionTree<Key> HybridBayesNet::error(
AlgebraicDecisionTree<Key> HybridBayesNet::logProbability(
const VectorValues &continuousValues) const {
AlgebraicDecisionTree<Key> error_tree(0.0);
AlgebraicDecisionTree<Key> result(0.0);
// Iterate over each conditional.
for (auto &&conditional : *this) {
if (auto gm = conditional->asMixture()) {
// If conditional is hybrid, select based on assignment and compute error.
AlgebraicDecisionTree<Key> conditional_error =
gm->error(continuousValues);
error_tree = error_tree + conditional_error;
// If conditional is hybrid, select based on assignment and compute
// logProbability.
result = result + gm->logProbability(continuousValues);
} else if (auto gc = conditional->asGaussian()) {
// If continuous only, get the (double) error
// and add it to the error_tree
double error = gc->error(continuousValues);
// Add the computed error to every leaf of the error tree.
error_tree = error_tree.apply(
[error](double leaf_value) { return leaf_value + error; });
// If continuous, get the (double) logProbability and add it to the
// result
double logProbability = gc->logProbability(continuousValues);
// Add the computed logProbability to every leaf of the logProbability
// tree.
result = result.apply([logProbability](double leaf_value) {
return leaf_value + logProbability;
});
} else if (auto dc = conditional->asDiscrete()) {
// Conditional is discrete-only, we skip.
// TODO(dellaert): if discrete, we need to add logProbability in the right
// branch?
continue;
}
}
return error_tree;
return result;
}
/* ************************************************************************* */
AlgebraicDecisionTree<Key> HybridBayesNet::probPrime(
AlgebraicDecisionTree<Key> HybridBayesNet::evaluate(
const VectorValues &continuousValues) const {
AlgebraicDecisionTree<Key> error_tree = this->error(continuousValues);
return error_tree.apply([](double error) { return exp(-error); });
AlgebraicDecisionTree<Key> tree = this->logProbability(continuousValues);
return tree.apply([](double log) { return exp(log); });
}
/* ************************************************************************* */
double HybridBayesNet::evaluate(const HybridValues &values) const {
return exp(logProbability(values));
}
/* ************************************************************************* */

16
gtsam/hybrid/HybridBayesNet.h
View File

@ -187,15 +187,6 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
/// Prune the Hybrid Bayes Net such that we have at most maxNrLeaves leaves.
HybridBayesNet prune(size_t maxNrLeaves);
/**
* @brief 0.5 * sum of squared Mahalanobis distances
* for a specific discrete assignment.
*
* @param values Continuous values and discrete assignment.
* @return double
*/
double error(const HybridValues &values) const;
/**
* @brief Compute conditional error for each discrete assignment,
* and return as a tree.
@ -203,7 +194,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
* @param continuousValues Continuous values at which to compute the error.
* @return AlgebraicDecisionTree<Key>
*/
AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
AlgebraicDecisionTree<Key> logProbability(
const VectorValues &continuousValues) const;
using BayesNet::logProbability; // expose HybridValues version
/**
* @brief Compute unnormalized probability q(μ|M),
@ -215,7 +209,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
* probability.
* @return AlgebraicDecisionTree<Key>
*/
AlgebraicDecisionTree<Key> probPrime(
AlgebraicDecisionTree<Key> evaluate(
const VectorValues &continuousValues) const;
/**

14
gtsam/hybrid/HybridConditional.cpp
View File

@ -122,18 +122,18 @@ bool HybridConditional::equals(const HybridFactor &other, double tol) const {
}
/* ************************************************************************ */
double HybridConditional::error(const HybridValues &values) const {
if (auto gm = asMixture()) {
return gm->error(values);
}
double HybridConditional::logProbability(const HybridValues &values) const {
if (auto gc = asGaussian()) {
return gc->error(values.continuous());
return gc->logProbability(values.continuous());
}
if (auto gm = asMixture()) {
return gm->logProbability(values);
}
if (auto dc = asDiscrete()) {
return -log((*dc)(values.discrete()));
return dc->logProbability(values.discrete());
}
throw std::runtime_error(
"HybridConditional::error: conditional type not handled");
"HybridConditional::logProbability: conditional type not handled");
}
} // namespace gtsam

4
gtsam/hybrid/HybridConditional.h
View File

@ -176,8 +176,8 @@ class GTSAM_EXPORT HybridConditional
/// Get the type-erased pointer to the inner type
boost::shared_ptr<Factor> inner() const { return inner_; }
/// Return the error of the underlying conditional.
double error(const HybridValues& values) const override;
/// Return the logProbability of the underlying conditional.
double logProbability(const HybridValues& values) const override;
/// Check if VectorValues `measurements` contains all frontal keys.
bool frontalsIn(const VectorValues& measurements) const {

9
gtsam/hybrid/HybridFactor.h
View File

@ -143,15 +143,6 @@ class GTSAM_EXPORT HybridFactor : public Factor {
/// @name Standard Interface
/// @{
/**
* @brief Compute the error of this Gaussian Mixture given the continuous
* values and a discrete assignment.
*
* @param values Continuous values and discrete assignment.
* @return double
*/
virtual double error(const HybridValues &values) const = 0;
/// True if this is a factor of discrete variables only.
bool isDiscrete() const { return isDiscrete_; }

8
gtsam/hybrid/HybridFactorGraph.cpp
View File

@ -25,17 +25,17 @@
namespace gtsam {
/* ************************************************************************* */
DiscreteKeys HybridFactorGraph::discreteKeys() const {
DiscreteKeys keys;
std::set<DiscreteKey> HybridFactorGraph::discreteKeys() const {
std::set<DiscreteKey> keys;
for (auto& factor : factors_) {
if (auto p = boost::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
for (const DiscreteKey& key : p->discreteKeys()) {
keys.push_back(key);
keys.insert(key);
}
}
if (auto p = boost::dynamic_pointer_cast<HybridFactor>(factor)) {
for (const DiscreteKey& key : p->discreteKeys()) {
keys.push_back(key);
keys.insert(key);
}
}
}

2
gtsam/hybrid/HybridFactorGraph.h
View File

@ -65,7 +65,7 @@ class HybridFactorGraph : public FactorGraph<Factor> {
/// @{
/// Get all the discrete keys in the factor graph.
DiscreteKeys discreteKeys() const;
std::set<DiscreteKey> discreteKeys() const;
/// Get all the discrete keys in the factor graph, as a set.
KeySet discreteKeySet() const;

18
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -463,24 +463,6 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::error(
return error_tree;
}
/* ************************************************************************ */
double HybridGaussianFactorGraph::error(const HybridValues &values) const {
double error = 0.0;
for (auto &f : factors_) {
if (auto hf = dynamic_pointer_cast<GaussianFactor>(f)) {
error += hf->error(values.continuous());
} else if (auto hf = dynamic_pointer_cast<HybridFactor>(f)) {
// TODO(dellaert): needs to change when we discard other wrappers.
error += hf->error(values);
} else if (auto dtf = dynamic_pointer_cast<DecisionTreeFactor>(f)) {
error -= log((*dtf)(values.discrete()));
} else {
throwRuntimeError("HybridGaussianFactorGraph::error(HV)", f);
}
}
return error;
}
/* ************************************************************************ */
double HybridGaussianFactorGraph::probPrime(const HybridValues &values) const {
double error = this->error(values);

10
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -145,6 +145,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
/// @name Standard Interface
/// @{
using Base::error; // Expose error(const HybridValues&) method..
/**
* @brief Compute error for each discrete assignment,
* and return as a tree.
@ -156,14 +158,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
*/
AlgebraicDecisionTree<Key> error(const VectorValues& continuousValues) const;
/**
* @brief Compute error given a continuous vector values
* and a discrete assignment.
*
* @return double
*/
double error(const HybridValues& values) const;
/**
* @brief Compute unnormalized probability \f$ P(X | M, Z) \f$
* for each discrete assignment, and return as a tree.

7
gtsam/hybrid/HybridNonlinearFactorGraph.h
View File

@ -55,12 +55,18 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
: Base(graph) {}
/// @}
/// @name Constructors
/// @{
/// Print the factor graph.
void print(
const std::string& s = "HybridNonlinearFactorGraph",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/// @}
/// @name Standard Interface
/// @{
/**
* @brief Linearize all the continuous factors in the
* HybridNonlinearFactorGraph.
@ -70,6 +76,7 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
*/
HybridGaussianFactorGraph::shared_ptr linearize(
const Values& continuousValues) const;
/// @}
};
template <>

70
gtsam/hybrid/HybridValues.h
View File

@ -37,12 +37,15 @@ namespace gtsam {
*/
class GTSAM_EXPORT HybridValues {
private:
// VectorValue stored the continuous components of the HybridValues.
/// Continuous multi-dimensional vectors for \class GaussianFactor.
VectorValues continuous_;
// DiscreteValue stored the discrete components of the HybridValues.
/// Discrete values for \class DiscreteFactor.
DiscreteValues discrete_;
/// Continuous, differentiable manifold values for \class NonlinearFactor.
Values nonlinear_;
public:
/// @name Standard Constructors
/// @{
@ -54,6 +57,11 @@ class GTSAM_EXPORT HybridValues {
HybridValues(const VectorValues& cv, const DiscreteValues& dv)
: continuous_(cv), discrete_(dv){};
/// Construct from all values types.
HybridValues(const VectorValues& cv, const DiscreteValues& dv,
const Values& v)
: continuous_(cv), discrete_(dv), nonlinear_(v){};
/// @}
/// @name Testable
/// @{
@ -77,26 +85,30 @@ class GTSAM_EXPORT HybridValues {
/// @name Interface
/// @{
/// Return the discrete MPE assignment
const DiscreteValues& discrete() const { return discrete_; }
/// Return the delta update for the continuous vectors
/// Return the multi-dimensional vector values.
const VectorValues& continuous() const { return continuous_; }
/// Check whether a variable with key \c j exists in DiscreteValue.
bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); };
/// Return the discrete values.
const DiscreteValues& discrete() const { return discrete_; }
/// Check whether a variable with key \c j exists in VectorValue.
/// Return the nonlinear values.
const Values& nonlinear() const { return nonlinear_; }
/// Check whether a variable with key \c j exists in VectorValues.
bool existsVector(Key j) { return continuous_.exists(j); };
/// Check whether a variable with key \c j exists.
bool exists(Key j) { return existsDiscrete(j) || existsVector(j); };
/// Check whether a variable with key \c j exists in DiscreteValues.
bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); };
/** Insert a discrete \c value with key \c j. Replaces the existing value if
* the key \c j is already used.
* @param value The vector to be inserted.
* @param j The index with which the value will be associated. */
void insert(Key j, size_t value) { discrete_[j] = value; };
/// Check whether a variable with key \c j exists in values.
bool existsNonlinear(Key j) {
return (nonlinear_.find(j) != nonlinear_.end());
};
/// Check whether a variable with key \c j exists.
bool exists(Key j) {
return existsVector(j) || existsDiscrete(j) || existsNonlinear(j);
};
/** Insert a vector \c value with key \c j. Throws an invalid_argument
* exception if the key \c j is already used.
@ -104,6 +116,12 @@ 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 a discrete \c value with key \c j. Replaces the existing value if
* the key \c j is already used.
* @param value The vector to be inserted.
* @param j The index with which the value will be associated. */
void insert(Key j, size_t value) { discrete_[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) {
@ -118,28 +136,36 @@ class GTSAM_EXPORT HybridValues {
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 Values& values) {
nonlinear_.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());
nonlinear_.insert(values.nonlinear());
return *this;
}
// TODO(Shangjie)- insert_or_assign() , similar to Values.h
/**
* Read/write access to the discrete value with key \c j, throws
* std::out_of_range if \c j does not exist.
*/
size_t& atDiscrete(Key j) { return discrete_.at(j); };
/**
* Read/write access to the vector value with key \c j, throws
* std::out_of_range if \c j does not exist.
*/
Vector& at(Key j) { return continuous_.at(j); };
/**
* Read/write access to the discrete value with key \c j, throws
* std::out_of_range if \c j does not exist.
*/
size_t& atDiscrete(Key j) { return discrete_.at(j); };
/** 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.

8
gtsam/hybrid/hybrid.i
View File

@ -61,6 +61,9 @@ virtual class HybridConditional {
size_t nrParents() const;
// Standard interface:
double logProbability(const gtsam::HybridValues& values) const;
double evaluate(const gtsam::HybridValues& values) const;
double operator()(const gtsam::HybridValues& values) const;
gtsam::GaussianMixture* asMixture() const;
gtsam::GaussianConditional* asGaussian() const;
gtsam::DiscreteConditional* asDiscrete() const;
@ -133,7 +136,10 @@ class HybridBayesNet {
gtsam::KeySet keys() const;
const gtsam::HybridConditional* at(size_t i) const;
double evaluate(const gtsam::HybridValues& x) const;
// Standard interface:
double logProbability(const gtsam::HybridValues& values) const;
double evaluate(const gtsam::HybridValues& values) const;
gtsam::HybridValues optimize() const;
gtsam::HybridValues sample(const gtsam::HybridValues &given) const;
gtsam::HybridValues sample() const;

14
gtsam/hybrid/tests/testGaussianMixture.cpp
View File

@ -116,11 +116,12 @@ TEST(GaussianMixture, Error) {
VectorValues values;
values.insert(X(1), Vector2::Ones());
values.insert(X(2), Vector2::Zero());
auto error_tree = mixture.error(values);
auto error_tree = mixture.logProbability(values);
// regression
// Check result.
std::vector<DiscreteKey> discrete_keys = {m1};
std::vector<double> leaves = {0.5, 4.3252595};
std::vector<double> leaves = {conditional0->logProbability(values),
conditional1->logProbability(values)};
AlgebraicDecisionTree<Key> expected_error(discrete_keys, leaves);
EXPECT(assert_equal(expected_error, error_tree, 1e-6));
@ -128,10 +129,11 @@ TEST(GaussianMixture, Error) {
// Regression for non-tree version.
DiscreteValues assignment;
assignment[M(1)] = 0;
EXPECT_DOUBLES_EQUAL(0.5, mixture.error({values, assignment}), 1e-8);
EXPECT_DOUBLES_EQUAL(conditional0->logProbability(values),
mixture.logProbability({values, assignment}), 1e-8);
assignment[M(1)] = 1;
EXPECT_DOUBLES_EQUAL(4.3252595155709335, mixture.error({values, assignment}),
1e-8);
EXPECT_DOUBLES_EQUAL(conditional1->logProbability(values),
mixture.logProbability({values, assignment}), 1e-8);
}
/* ************************************************************************* */

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

@ -64,10 +64,10 @@ TEST(HybridBayesNet, Add) {
// Test evaluate for a pure discrete Bayes net P(Asia).
TEST(HybridBayesNet, EvaluatePureDiscrete) {
HybridBayesNet bayesNet;
bayesNet.emplace_back(new DiscreteConditional(Asia, "99/1"));
bayesNet.emplace_back(new DiscreteConditional(Asia, "4/6"));
HybridValues values;
values.insert(asiaKey, 0);
EXPECT_DOUBLES_EQUAL(0.99, bayesNet.evaluate(values), 1e-9);
EXPECT_DOUBLES_EQUAL(0.4, bayesNet.evaluate(values), 1e-9);
}
/* ****************************************************************************/
@ -207,55 +207,57 @@ TEST(HybridBayesNet, Optimize) {
/* ****************************************************************************/
// Test Bayes net error
TEST(HybridBayesNet, Error) {
TEST(HybridBayesNet, logProbability) {
Switching s(3);
HybridBayesNet::shared_ptr hybridBayesNet =
s.linearizedFactorGraph.eliminateSequential();
EXPECT_LONGS_EQUAL(5, hybridBayesNet->size());
HybridValues delta = hybridBayesNet->optimize();
auto error_tree = hybridBayesNet->error(delta.continuous());
auto error_tree = hybridBayesNet->logProbability(delta.continuous());
std::vector<DiscreteKey> discrete_keys = {{M(0), 2}, {M(1), 2}};
std::vector<double> leaves = {0.0097568009, 3.3973404e-31, 0.029126214,
0.0097568009};
std::vector<double> leaves = {4.1609374, 4.1706942, 4.141568, 4.1609374};
AlgebraicDecisionTree<Key> expected_error(discrete_keys, leaves);
// regression
EXPECT(assert_equal(expected_error, error_tree, 1e-9));
EXPECT(assert_equal(expected_error, error_tree, 1e-6));
// Error on pruned Bayes net
// logProbability on pruned Bayes net
auto prunedBayesNet = hybridBayesNet->prune(2);
auto pruned_error_tree = prunedBayesNet.error(delta.continuous());
auto pruned_error_tree = prunedBayesNet.logProbability(delta.continuous());
std::vector<double> pruned_leaves = {2e50, 3.3973404e-31, 2e50, 0.0097568009};
std::vector<double> pruned_leaves = {2e50, 4.1706942, 2e50, 4.1609374};
AlgebraicDecisionTree<Key> expected_pruned_error(discrete_keys,
pruned_leaves);
// regression
EXPECT(assert_equal(expected_pruned_error, pruned_error_tree, 1e-9));
EXPECT(assert_equal(expected_pruned_error, pruned_error_tree, 1e-6));
// Verify error computation and check for specific error value
DiscreteValues discrete_values{{M(0), 1}, {M(1), 1}};
// Verify logProbability computation and check for specific logProbability
// value
const DiscreteValues discrete_values{{M(0), 1}, {M(1), 1}};
const HybridValues hybridValues{delta.continuous(), discrete_values};
double logProbability = 0;
logProbability +=
hybridBayesNet->at(0)->asMixture()->logProbability(hybridValues);
logProbability +=
hybridBayesNet->at(1)->asMixture()->logProbability(hybridValues);
logProbability +=
hybridBayesNet->at(2)->asMixture()->logProbability(hybridValues);
double total_error = 0;
for (size_t idx = 0; idx < hybridBayesNet->size(); idx++) {
if (hybridBayesNet->at(idx)->isHybrid()) {
double error = hybridBayesNet->at(idx)->asMixture()->error(
{delta.continuous(), discrete_values});
total_error += error;
} else if (hybridBayesNet->at(idx)->isContinuous()) {
double error =
hybridBayesNet->at(idx)->asGaussian()->error(delta.continuous());
total_error += error;
}
}
EXPECT_DOUBLES_EQUAL(
total_error, hybridBayesNet->error({delta.continuous(), discrete_values}),
// TODO(dellaert): the discrete errors are not added in logProbability tree!
EXPECT_DOUBLES_EQUAL(logProbability, error_tree(discrete_values), 1e-9);
EXPECT_DOUBLES_EQUAL(logProbability, pruned_error_tree(discrete_values),
1e-9);
EXPECT_DOUBLES_EQUAL(total_error, error_tree(discrete_values), 1e-9);
EXPECT_DOUBLES_EQUAL(total_error, pruned_error_tree(discrete_values), 1e-9);
logProbability +=
hybridBayesNet->at(3)->asDiscrete()->logProbability(discrete_values);
logProbability +=
hybridBayesNet->at(4)->asDiscrete()->logProbability(discrete_values);
EXPECT_DOUBLES_EQUAL(logProbability,
hybridBayesNet->logProbability(hybridValues), 1e-9);
}
/* ****************************************************************************/

10
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

@ -60,12 +60,14 @@ TEST(HybridFactorGraph, GaussianFactorGraph) {
Values linearizationPoint;
linearizationPoint.insert<double>(X(0), 0);
// Linearize the factor graph.
HybridGaussianFactorGraph ghfg = *fg.linearize(linearizationPoint);
EXPECT_LONGS_EQUAL(1, ghfg.size());
// Add a factor to the GaussianFactorGraph
ghfg.add(JacobianFactor(X(0), I_1x1, Vector1(5)));
EXPECT_LONGS_EQUAL(2, ghfg.size());
// Check that the error is the same for the nonlinear values.
const VectorValues zero{{X(0), Vector1(0)}};
const HybridValues hybridValues{zero, {}, linearizationPoint};
EXPECT_DOUBLES_EQUAL(fg.error(hybridValues), ghfg.error(hybridValues), 1e-9);
}
/***************************************************************************

16
gtsam/inference/BayesNet-inst.h
View File

@ -88,6 +88,22 @@ void BayesNet<CONDITIONAL>::saveGraph(const std::string& filename,
of.close();
}
/* ************************************************************************* */
template <class CONDITIONAL>
double BayesNet<CONDITIONAL>::logProbability(const HybridValues& x) const {
double sum = 0.;
for (const auto& gc : *this) {
if (gc) sum += gc->logProbability(x);
}
return sum;
}
/* ************************************************************************* */
template <class CONDITIONAL>
double BayesNet<CONDITIONAL>::evaluate(const HybridValues& x) const {
return exp(-logProbability(x));
}
/* ************************************************************************* */
} // namespace gtsam

19
gtsam/inference/BayesNet.h
View File

@ -25,6 +25,8 @@
namespace gtsam {
class HybridValues;
/**
* A BayesNet is a tree of conditionals, stored in elimination order.
* @ingroup inference
@ -52,9 +54,11 @@ class BayesNet : public FactorGraph<CONDITIONAL> {
/**
* Constructor that takes an initializer list of shared pointers.
* BayesNet<SymbolicConditional> bn = {make_shared<SymbolicConditional>(), ...};
* BayesNet<SymbolicConditional> bn = {make_shared<SymbolicConditional>(),
* ...};
*/
BayesNet(std::initializer_list<sharedConditional> conditionals): Base(conditionals) {}
BayesNet(std::initializer_list<sharedConditional> conditionals)
: Base(conditionals) {}
/// @}
@ -68,7 +72,6 @@ class BayesNet : public FactorGraph<CONDITIONAL> {
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/// @}
/// @name Graph Display
/// @{
@ -86,6 +89,16 @@ class BayesNet : public FactorGraph<CONDITIONAL> {
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const DotWriter& writer = DotWriter()) const;
/// @}
/// @name HybridValues methods
/// @{
// Expose HybridValues version of logProbability.
double logProbability(const HybridValues& x) const;
// Expose HybridValues version of evaluate.
double evaluate(const HybridValues& c) const;
/// @}
};

38
gtsam/inference/Conditional-inst.h
View File

@ -18,30 +18,42 @@
// \callgraph
#pragma once
#include <iostream>
#include <gtsam/inference/Conditional.h>
#include <cmath>
#include <iostream>
namespace gtsam {
/* ************************************************************************* */
template<class FACTOR, class DERIVEDFACTOR>
void Conditional<FACTOR,DERIVEDFACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
template <class FACTOR, class DERIVEDCONDITIONAL>
void Conditional<FACTOR, DERIVEDCONDITIONAL>::print(
const std::string& s, const KeyFormatter& formatter) const {
std::cout << s << " P(";
for(Key key: frontals())
std::cout << " " << formatter(key);
if (nrParents() > 0)
std::cout << " |";
for(Key parent: parents())
std::cout << " " << formatter(parent);
for (Key key : frontals()) std::cout << " " << formatter(key);
if (nrParents() > 0) std::cout << " |";
for (Key parent : parents()) std::cout << " " << formatter(parent);
std::cout << ")" << std::endl;
}
/* ************************************************************************* */
template<class FACTOR, class DERIVEDFACTOR>
bool Conditional<FACTOR,DERIVEDFACTOR>::equals(const This& c, double tol) const
{
template <class FACTOR, class DERIVEDCONDITIONAL>
bool Conditional<FACTOR, DERIVEDCONDITIONAL>::equals(const This& c,
double tol) const {
return nrFrontals_ == c.nrFrontals_;
}
/* ************************************************************************* */
template <class FACTOR, class DERIVEDCONDITIONAL>
double Conditional<FACTOR, DERIVEDCONDITIONAL>::logProbability(
const HybridValues& c) const {
throw std::runtime_error("Conditional::logProbability is not implemented");
}
/* ************************************************************************* */
template <class FACTOR, class DERIVEDCONDITIONAL>
double Conditional<FACTOR, DERIVEDCONDITIONAL>::evaluate(
const HybridValues& c) const {
throw std::runtime_error("Conditional::evaluate is not implemented");
}
} // namespace gtsam

55
gtsam/inference/Conditional.h
View File

@ -24,13 +24,37 @@
namespace gtsam {
class HybridValues; // forward declaration.
/**
* Base class for conditional densities. This class iterators and
* access to the frontal and separator keys.
* This is the base class for all conditional distributions/densities,
* which are implemented as specialized factors. This class does not store any
* data other than its keys. Derived classes store data such as matrices and
* probability tables.
*
* The `evaluate` method is used to evaluate the factor, and together with
* `logProbability` is the main methods that need to be implemented in derived
* classes. These two methods relate to the `error` method in the factor by:
* probability(x) = k exp(-error(x))
* where k is a normalization constant making \int probability(x) == 1.0, and
* logProbability(x) = K - error(x)
* i.e., K = log(K).
*
* There are four broad classes of conditionals that derive from Conditional:
*
* - \b Gaussian conditionals, implemented in \class GaussianConditional, a
* Gaussian density over a set of continuous variables.
* - \b Discrete conditionals, implemented in \class DiscreteConditional, which
* represent a discrete conditional distribution over discrete variables.
* - \b Hybrid conditional densities, such as \class GaussianMixture, which is
* a density over continuous variables given discrete/continuous parents.
* - \b Symbolic factors, used to represent a graph structure, implemented in
* \class SymbolicConditional. Only used for symbolic elimination etc.
*
* Derived classes *must* redefine the Factor and shared_ptr typedefs to refer
* to the associated factor type and shared_ptr type of the derived class. See
* SymbolicConditional and GaussianConditional for examples.
*
* \nosubgrouping
*/
template<class FACTOR, class DERIVEDCONDITIONAL>
@ -78,6 +102,8 @@ namespace gtsam {
/// @name Standard Interface
/// @{
virtual ~Conditional() {}
/** return the number of frontals */
size_t nrFrontals() const { return nrFrontals_; }
@ -98,6 +124,27 @@ namespace gtsam {
/** return a view of the parent keys */
Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); }
/**
* All conditional types need to implement a `logProbability` function, for which
* exp(logProbability(x)) = evaluate(x).
*/
virtual double logProbability(const HybridValues& c) const;
/**
* All conditional types need to implement an `evaluate` function, that yields
* a true probability. The default implementation just exponentiates logProbability.
*/
virtual double evaluate(const HybridValues& c) const;
/// Evaluate probability density, sugar.
double operator()(const HybridValues& x) const {
return evaluate(x);
}
/// @}
/// @name Advanced Interface
/// @{
/** Iterator pointing to first frontal key. */
typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); }
@ -110,10 +157,6 @@ namespace gtsam {
/** Iterator pointing past the last parent key. */
typename FACTOR::const_iterator endParents() const { return asFactor().end(); }
/// @}
/// @name Advanced Interface
/// @{
/** Mutable version of nrFrontals */
size_t& nrFrontals() { return nrFrontals_; }

6
gtsam/inference/Factor.cpp
View File

@ -43,4 +43,10 @@ namespace gtsam {
return keys_ == other.keys_;
}
/* ************************************************************************* */
double Factor::error(const HybridValues& c) const {
throw std::runtime_error("Factor::error is not implemented");
}
}

44
gtsam/inference/Factor.h
View File

@ -29,21 +29,38 @@
#include <gtsam/inference/Key.h>
namespace gtsam {
/// Define collection types:
typedef FastVector<FactorIndex> FactorIndices;
typedef FastSet<FactorIndex> FactorIndexSet;
class HybridValues; // forward declaration of a Value type for error.
/**
* This is the base class for all factor types. This class does not store any
* This is the base class for all factor types, as well as conditionals,
* which are implemented as specialized factors. This class does not store any
* data other than its keys. Derived classes store data such as matrices and
* probability tables.
*
* Note that derived classes *must* redefine the `This` and `shared_ptr`
* typedefs. See JacobianFactor, etc. for examples.
* The `error` method is used to evaluate the factor, and is the only method
* that is required to be implemented in derived classes, although it has a
* default implementation that throws an exception.
*
* This class is \b not virtual for performance reasons - the derived class
* SymbolicFactor needs to be created and destroyed quickly during symbolic
* elimination. GaussianFactor and NonlinearFactor are virtual.
* There are five broad classes of factors that derive from Factor:
*
* - \b Nonlinear factors, such as \class NonlinearFactor and \class NoiseModelFactor, which
* represent a nonlinear likelihood function over a set of variables.
* - \b Gaussian factors, such as \class JacobianFactor and \class HessianFactor, which
* represent a Gaussian likelihood over a set of variables.
* - \b Discrete factors, such as \class DiscreteFactor and \class DecisionTreeFactor, which
* represent a discrete distribution over a set of variables.
* - \b Hybrid factors, such as \class HybridFactor, which represent a mixture of
* Gaussian and discrete distributions over a set of variables.
* - \b Symbolic factors, used to represent a graph structure, such as
* \class SymbolicFactor, only used for symbolic elimination etc.
*
* Note that derived classes must also redefine the `This` and `shared_ptr`
* typedefs. See JacobianFactor, etc. for examples.
*
* \nosubgrouping
*/
@ -128,6 +145,12 @@ typedef FastSet<FactorIndex> FactorIndexSet;
/** Iterator at end of involved variable keys */
const_iterator end() const { return keys_.end(); }
/**
* All factor types need to implement an error function.
* In factor graphs, this is the negative log-likelihood.
*/
virtual double error(const HybridValues& c) const;
/**
* @return the number of variables involved in this factor
*/
@ -152,7 +175,6 @@ typedef FastSet<FactorIndex> FactorIndexSet;
bool equals(const This& other, double tol = 1e-9) const;
/// @}
/// @name Advanced Interface
/// @{
@ -165,7 +187,13 @@ typedef FastSet<FactorIndex> FactorIndexSet;
/** Iterator at end of involved variable keys */
iterator end() { return keys_.end(); }
/// @}
private:
/// @name Serialization
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
@ -177,4 +205,4 @@ typedef FastSet<FactorIndex> FactorIndexSet;
};
}
} // \namespace gtsam

10
gtsam/inference/FactorGraph-inst.h
View File

@ -61,6 +61,16 @@ bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
return true;
}
/* ************************************************************************ */
template <class FACTOR>
double FactorGraph<FACTOR>::error(const HybridValues &values) const {
double error = 0.0;
for (auto &f : factors_) {
error += f->error(values);
}
return error;
}
/* ************************************************************************* */
template <class FACTOR>
size_t FactorGraph<FACTOR>::nrFactors() const {

5
gtsam/inference/FactorGraph.h
View File

@ -47,6 +47,8 @@ typedef FastVector<FactorIndex> FactorIndices;
template <class CLIQUE>
class BayesTree;
class HybridValues;
/** Helper */
template <class C>
class CRefCallPushBack {
@ -359,6 +361,9 @@ class FactorGraph {
/** Get the last factor */
sharedFactor back() const { return factors_.back(); }
/** Add error for all factors. */
double error(const HybridValues &values) const;
/// @}
/// @name Modifying Factor Graphs (imperative, discouraged)
/// @{

34
gtsam/linear/GaussianBayesNet.cpp
View File

@ -104,7 +104,25 @@ namespace gtsam {
/* ************************************************************************* */
double GaussianBayesNet::error(const VectorValues& x) const {
return GaussianFactorGraph(*this).error(x);
double sum = 0.;
for (const auto& gc : *this) {
if (gc) sum += gc->error(x);
}
return sum;
}
/* ************************************************************************* */
double GaussianBayesNet::logProbability(const VectorValues& x) const {
double sum = 0.;
for (const auto& gc : *this) {
if (gc) sum += gc->logProbability(x);
}
return sum;
}
/* ************************************************************************* */
double GaussianBayesNet::evaluate(const VectorValues& x) const {
return exp(logProbability(x));
}
/* ************************************************************************* */
@ -225,19 +243,5 @@ namespace gtsam {
}
/* ************************************************************************* */
double GaussianBayesNet::logDensity(const VectorValues& x) const {
double sum = 0.0;
for (const auto& conditional : *this) {
if (conditional) sum += conditional->logDensity(x);
}
return sum;
}
/* ************************************************************************* */
double GaussianBayesNet::evaluate(const VectorValues& x) const {
return exp(logDensity(x));
}
/* ************************************************************************* */
} // namespace gtsam

29
gtsam/linear/GaussianBayesNet.h
View File

@ -97,11 +97,16 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/// Sum error over all variables.
double error(const VectorValues& x) const;
/// Sum logProbability over all variables.
double logProbability(const VectorValues& x) const;
/**
* Calculate probability density for given values `x`:
* exp(-error(x)) / sqrt((2*pi)^n*det(Sigma))
* where x is the vector of values, and Sigma is the covariance matrix.
* Note that error(x)=0.5*e'*e includes the 0.5 factor already.
* exp(logProbability)
* where x is the vector of values.
*/
double evaluate(const VectorValues& x) const;
@ -110,13 +115,6 @@ namespace gtsam {
return evaluate(x);
}
/**
* Calculate log-density for given values `x`:
* -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
* where x is the vector of values, and Sigma is the covariance matrix.
*/
double logDensity(const VectorValues& x) const;
/// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by
/// back-substitution
VectorValues optimize() const;
@ -216,9 +214,6 @@ namespace gtsam {
* allocateVectorValues */
VectorValues gradientAtZero() const;
/** 0.5 * sum of squared Mahalanobis distances. */
double error(const VectorValues& x) const;
/**
* Computes the determinant of a GassianBayesNet. A GaussianBayesNet is an upper triangular
* matrix and for an upper triangular matrix determinant is the product of the diagonal
@ -251,6 +246,14 @@ namespace gtsam {
VectorValues backSubstituteTranspose(const VectorValues& gx) const;
/// @}
/// @name HybridValues methods.
/// @{
using Base::evaluate; // Expose evaluate(const HybridValues&) method..
using Base::logProbability; // Expose logProbability(const HybridValues&) method..
using Base::error; // Expose error(const HybridValues&) method..
/// @}
private:
/** Serialization function */

13
gtsam/linear/GaussianConditional.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/linear/Sampler.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/hybrid/HybridValues.h>
#include <boost/format.hpp>
#ifdef __GNUC__
@ -34,6 +35,7 @@
#include <functional>
#include <list>
#include <string>
#include <cmath>
// In Wrappers we have no access to this so have a default ready
static std::mt19937_64 kRandomNumberGenerator(42);
@ -194,15 +196,18 @@ double GaussianConditional::logNormalizationConstant() const {
/* ************************************************************************* */
// density = k exp(-error(x))
// log = log(k) - error(x)
double GaussianConditional::logDensity(const VectorValues& x) const {
double GaussianConditional::logProbability(const VectorValues& x) const {
return logNormalizationConstant() - error(x);
}
/* ************************************************************************* */
double GaussianConditional::evaluate(const VectorValues& x) const {
return exp(logDensity(x));
double GaussianConditional::logProbability(const HybridValues& x) const {
return logProbability(x.continuous());
}
/* ************************************************************************* */
double GaussianConditional::evaluate(const VectorValues& c) const {
return exp(logProbability(c));
}
/* ************************************************************************* */
VectorValues GaussianConditional::solve(const VectorValues& x) const {
// Concatenate all vector values that correspond to parent variables

136
gtsam/linear/GaussianConditional.h
View File

@ -132,64 +132,6 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/**
* Calculate probability density for given values `x`:
* exp(-error(x)) / sqrt((2*pi)^n*det(Sigma))
* where x is the vector of values, and Sigma is the covariance matrix.
* Note that error(x)=0.5*e'*e includes the 0.5 factor already.
*/
double evaluate(const VectorValues& x) const;
/// Evaluate probability density, sugar.
double operator()(const VectorValues& x) const {
return evaluate(x);
}
/**
* Calculate log-density for given values `x`:
* -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
* where x is the vector of values, and Sigma is the covariance matrix.
*/
double logDensity(const VectorValues& x) const;
/** Return a view of the upper-triangular R block of the conditional */
constABlock R() const { return Ab_.range(0, nrFrontals()); }
/** Get a view of the parent blocks. */
constABlock S() const { return Ab_.range(nrFrontals(), size()); }
/** Get a view of the S matrix for the variable pointed to by the given key iterator */
constABlock S(const_iterator it) const { return BaseFactor::getA(it); }
/** Get a view of the r.h.s. vector d */
const constBVector d() const { return BaseFactor::getb(); }
/**
* @brief Compute the determinant of the R matrix.
*
* The determinant is computed in log form using logDeterminant for
* numerical stability and then exponentiated.
*
* Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence
* \f$ \det(\Sigma) = 1 / \det(R^T R) = 1 / determinant()^ 2 \f$.
*
* @return double
*/
inline double determinant() const { return exp(logDeterminant()); }
/**
* @brief Compute the log determinant of the R matrix.
*
* For numerical stability, the determinant is computed in log
* form, so it is a summation rather than a multiplication.
*
* Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence
* \f$ \log \det(\Sigma) = - \log \det(R^T R) = - 2 logDeterminant() \f$.
*
* @return double
*/
double logDeterminant() const;
/**
* normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
* log = - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
@ -203,6 +145,27 @@ namespace gtsam {
return exp(logNormalizationConstant());
}
/**
* Calculate log-probability log(evaluate(x)) for given values `x`:
* -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
* where x is the vector of values, and Sigma is the covariance matrix.
* This differs from error as it is log, not negative log, and it
* includes the normalization constant.
*/
double logProbability(const VectorValues& x) const;
/**
* Calculate probability density for given values `x`:
* exp(logProbability(x)) == exp(-GaussianFactor::error(x)) / sqrt((2*pi)^n*det(Sigma))
* where x is the vector of values, and Sigma is the covariance matrix.
*/
double evaluate(const VectorValues& x) const;
/// Evaluate probability density, sugar.
double operator()(const VectorValues& x) const {
return evaluate(x);
}
/**
* Solves a conditional Gaussian and writes the solution into the entries of
* \c x for each frontal variable of the conditional. The parents are
@ -255,6 +218,63 @@ namespace gtsam {
VectorValues sample(const VectorValues& parentsValues) const;
/// @}
/// @name Linear algebra.
/// @{
/** Return a view of the upper-triangular R block of the conditional */
constABlock R() const { return Ab_.range(0, nrFrontals()); }
/** Get a view of the parent blocks. */
constABlock S() const { return Ab_.range(nrFrontals(), size()); }
/** Get a view of the S matrix for the variable pointed to by the given key iterator */
constABlock S(const_iterator it) const { return BaseFactor::getA(it); }
/** Get a view of the r.h.s. vector d */
const constBVector d() const { return BaseFactor::getb(); }
/**
* @brief Compute the determinant of the R matrix.
*
* The determinant is computed in log form using logDeterminant for
* numerical stability and then exponentiated.
*
* Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence
* \f$ \det(\Sigma) = 1 / \det(R^T R) = 1 / determinant()^ 2 \f$.
*
* @return double
*/
inline double determinant() const { return exp(logDeterminant()); }
/**
* @brief Compute the log determinant of the R matrix.
*
* For numerical stability, the determinant is computed in log
* form, so it is a summation rather than a multiplication.
*
* Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence
* \f$ \log \det(\Sigma) = - \log \det(R^T R) = - 2 logDeterminant() \f$.
*
* @return double
*/
double logDeterminant() const;
/// @}
/// @name HybridValues methods.
/// @{
/**
* Calculate log-probability log(evaluate(x)) for HybridValues `x`.
* Simply dispatches to VectorValues version.
*/
double logProbability(const HybridValues& x) const override;
using Conditional::evaluate; // Expose evaluate(const HybridValues&) method..
using Conditional::operator(); // Expose evaluate(const HybridValues&) method..
using Base::error; // Expose error(const HybridValues&) method..
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// @name Deprecated

12
gtsam/linear/GaussianFactor.cpp
View File

@ -18,16 +18,24 @@
// \callgraph
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam {
/* ************************************************************************* */
double GaussianFactor::error(const VectorValues& c) const {
throw std::runtime_error("GaussianFactor::error is not implemented");
}
double GaussianFactor::error(const HybridValues& c) const {
return this->error(c.continuous());
}
VectorValues GaussianFactor::hessianDiagonal() const {
VectorValues d;
hessianDiagonalAdd(d);
return d;
}
}
} // namespace gtsam

16
gtsam/linear/GaussianFactor.h
View File

@ -63,8 +63,20 @@ namespace gtsam {
/** Equals for testable */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0;
/** Print for testable */
virtual double error(const VectorValues& c) const = 0; /** 0.5*(A*x-b)'*D*(A*x-b) */
/**
* In Gaussian factors, the error function returns either the negative log-likelihood, e.g.,
* 0.5*(A*x-b)'*D*(A*x-b)
* for a \class JacobianFactor, or the negative log-density, e.g.,
* 0.5*(A*x-b)'*D*(A*x-b) - log(k)
* for a \class GaussianConditional, where k is the normalization constant.
*/
virtual double error(const VectorValues& c) const;
/**
* The Factor::error simply extracts the \class VectorValues from the
* \class HybridValues and calculates the error.
*/
double error(const HybridValues& c) const override;
/** Return the dimension of the variable pointed to by the given key iterator */
virtual DenseIndex getDim(const_iterator variable) const = 0;

16
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -67,6 +67,22 @@ namespace gtsam {
return spec;
}
/* ************************************************************************* */
double GaussianFactorGraph::error(const VectorValues& x) const {
double total_error = 0.;
for(const sharedFactor& factor: *this){
if(factor)
total_error += factor->error(x);
}
return total_error;
}
/* ************************************************************************* */
double GaussianFactorGraph::probPrime(const VectorValues& c) const {
// NOTE the 0.5 constant is handled by the factor error.
return exp(-error(c));
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr GaussianFactorGraph::cloneToPtr() const {
gtsam::GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());

14
gtsam/linear/GaussianFactorGraph.h
View File

@ -167,20 +167,10 @@ namespace gtsam {
std::map<Key, size_t> getKeyDimMap() const;
/** unnormalized error */
double error(const VectorValues& x) const {
double total_error = 0.;
for(const sharedFactor& factor: *this){
if(factor)
total_error += factor->error(x);
}
return total_error;
}
double error(const VectorValues& x) const;
/** Unnormalized probability. O(n) */
double probPrime(const VectorValues& c) const {
// NOTE the 0.5 constant is handled by the factor error.
return exp(-error(c));
}
double probPrime(const VectorValues& c) const;
/**
* Clone() performs a deep-copy of the graph, including all of the factors.

7
gtsam/linear/linear.i
View File

@ -497,8 +497,9 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
bool equals(const gtsam::GaussianConditional& cg, double tol) const;
// Standard Interface
double logProbability(const gtsam::VectorValues& x) const;
double evaluate(const gtsam::VectorValues& x) const;
double logDensity(const gtsam::VectorValues& x) const;
double error(const gtsam::VectorValues& x) const;
gtsam::Key firstFrontalKey() const;
gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
gtsam::JacobianFactor* likelihood(
@ -558,8 +559,10 @@ virtual class GaussianBayesNet {
gtsam::GaussianConditional* back() const;
// Standard interface
// Standard Interface
double logProbability(const gtsam::VectorValues& x) const;
double evaluate(const gtsam::VectorValues& x) const;
double logDensity(const gtsam::VectorValues& x) const;
double error(const gtsam::VectorValues& x) const;
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(const gtsam::VectorValues& given) const;

12
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -78,9 +78,13 @@ TEST(GaussianBayesNet, Evaluate1) {
// which at the mean is 1.0! So, the only thing we need to calculate is
// the normalization constant 1.0/sqrt((2*pi*Sigma).det()).
// The covariance matrix inv(Sigma) = R'*R, so the determinant is
const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
const double constant = sqrt((invSigma / (2 * M_PI)).determinant());
EXPECT_DOUBLES_EQUAL(log(constant),
smallBayesNet.at(0)->logNormalizationConstant() +
smallBayesNet.at(1)->logNormalizationConstant(),
1e-9);
const double actual = smallBayesNet.evaluate(mean);
EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
EXPECT_DOUBLES_EQUAL(constant, actual, 1e-9);
}
// Check the evaluate with non-unit noise.
@ -89,9 +93,9 @@ TEST(GaussianBayesNet, Evaluate2) {
const VectorValues mean = noisyBayesNet.optimize();
const Matrix R = noisyBayesNet.matrix().first;
const Matrix invSigma = R.transpose() * R;
const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
const double constant = sqrt((invSigma / (2 * M_PI)).determinant());
const double actual = noisyBayesNet.evaluate(mean);
EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
EXPECT_DOUBLES_EQUAL(constant, actual, 1e-9);
}
/* ************************************************************************* */

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

@ -384,11 +384,17 @@ TEST(GaussianConditional, FromMeanAndStddev) {
double expected1 = 0.5 * e1.dot(e1);
EXPECT_DOUBLES_EQUAL(expected1, conditional1.error(values), 1e-9);
double expected2 = conditional1.logNormalizationConstant() - 0.5 * e1.dot(e1);
EXPECT_DOUBLES_EQUAL(expected2, conditional1.logProbability(values), 1e-9);
auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), A2,
X(2), b, sigma);
Vector2 e2 = (x0 - (A1 * x1 + A2 * x2 + b)) / sigma;
double expected2 = 0.5 * e2.dot(e2);
EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9);
double expected3 = 0.5 * e2.dot(e2);
EXPECT_DOUBLES_EQUAL(expected3, conditional2.error(values), 1e-9);
double expected4 = conditional2.logNormalizationConstant() - 0.5 * e2.dot(e2);
EXPECT_DOUBLES_EQUAL(expected4, conditional2.logProbability(values), 1e-9);
}
/* ************************************************************************* */
@ -448,20 +454,24 @@ TEST(GaussianConditional, sample) {
}
/* ************************************************************************* */
TEST(GaussianConditional, LogNormalizationConstant) {
TEST(GaussianConditional, Error) {
// Create univariate standard gaussian conditional
auto std_gaussian =
auto stdGaussian =
GaussianConditional::FromMeanAndStddev(X(0), Vector1::Zero(), 1.0);
VectorValues values;
values.insert(X(0), Vector1::Zero());
double logDensity = std_gaussian.logDensity(values);
double logProbability = stdGaussian.logProbability(values);
// Regression.
// These values were computed by hand for a univariate standard gaussian.
EXPECT_DOUBLES_EQUAL(-0.9189385332046727, logDensity, 1e-9);
EXPECT_DOUBLES_EQUAL(0.3989422804014327, exp(logDensity), 1e-9);
EXPECT_DOUBLES_EQUAL(-0.9189385332046727, logProbability, 1e-9);
EXPECT_DOUBLES_EQUAL(0.3989422804014327, exp(logProbability), 1e-9);
EXPECT_DOUBLES_EQUAL(stdGaussian(values), exp(logProbability), 1e-9);
}
/* ************************************************************************* */
// Similar test for multivariate gaussian but with sigma 2.0
TEST(GaussianConditional, LogNormalizationConstant) {
double sigma = 2.0;
auto conditional = GaussianConditional::FromMeanAndStddev(X(0), Vector3::Zero(), sigma);
VectorValues x;
@ -469,7 +479,8 @@ TEST(GaussianConditional, LogNormalizationConstant) {
Matrix3 Sigma = I_3x3 * sigma * sigma;
double expectedLogNormalizingConstant = log(1 / sqrt((2 * M_PI * Sigma).determinant()));
EXPECT_DOUBLES_EQUAL(expectedLogNormalizingConstant, conditional.logNormalizationConstant(), 1e-9);
EXPECT_DOUBLES_EQUAL(expectedLogNormalizingConstant,
conditional.logNormalizationConstant(), 1e-9);
}
/* ************************************************************************* */

7
gtsam/linear/tests/testGaussianDensity.cpp
View File

@ -52,8 +52,11 @@ TEST(GaussianDensity, FromMeanAndStddev) {
auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma);
Vector2 e = (x0 - b) / sigma;
double expected = 0.5 * e.dot(e);
EXPECT_DOUBLES_EQUAL(expected, density.error(values), 1e-9);
double expected1 = 0.5 * e.dot(e);
EXPECT_DOUBLES_EQUAL(expected1, density.error(values), 1e-9);
double expected2 = density.logNormalizationConstant()- 0.5 * e.dot(e);
EXPECT_DOUBLES_EQUAL(expected2, density.logProbability(values), 1e-9);
}
/* ************************************************************************* */

11
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -16,12 +16,23 @@
* @author Richard Roberts
*/
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <boost/make_shared.hpp>
#include <boost/format.hpp>
namespace gtsam {
/* ************************************************************************* */
double NonlinearFactor::error(const Values& c) const {
throw std::runtime_error("NonlinearFactor::error is not implemented");
}
/* ************************************************************************* */
double NonlinearFactor::error(const HybridValues& c) const {
return this->error(c.nonlinear());
}
/* ************************************************************************* */
void NonlinearFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const {

21
gtsam/nonlinear/NonlinearFactor.h
View File

@ -74,6 +74,7 @@ public:
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
@ -81,13 +82,25 @@ public:
/** Destructor */
virtual ~NonlinearFactor() {}
/**
* In nonlinear factors, the error function returns the negative log-likelihood
* as a non-linear function of the values in a \class Values object.
*
* The idea is that Gaussian factors have a quadratic error function that locally
* approximates the negative log-likelihood, and are obtained by \b linearizing
* the nonlinear error function at a given linearization.
*
* The derived class, \class NoiseModelFactor, adds a noise model to the factor,
* and calculates the error by asking the user to implement the method
* \code double evaluateError(const Values& c) const \endcode.
*/
virtual double error(const Values& c) const;
/**
* Calculate the error of the factor
* This is typically equal to log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/sigma^2 \f$ in case of Gaussian.
* You can override this for systems with unusual noise models.
* The Factor::error simply extracts the \class Values from the
* \class HybridValues and calculates the error.
*/
virtual double error(const Values& c) const = 0;
double error(const HybridValues& c) const override;
/** get the dimension of the factor (number of rows on linearization) */
virtual size_t dim() const = 0;

12
gtsam/symbolic/SymbolicConditional.cpp
View File

@ -15,7 +15,6 @@
* @date Oct 17, 2010
*/
#include <gtsam/inference/Conditional-inst.h>
#include <gtsam/symbolic/SymbolicConditional.h>
namespace gtsam {
@ -33,4 +32,15 @@ bool SymbolicConditional::equals(const This& c, double tol) const {
return BaseFactor::equals(c) && BaseConditional::equals(c);
}
/* ************************************************************************* */
double SymbolicConditional::logProbability(const HybridValues& c) const {
throw std::runtime_error("SymbolicConditional::logProbability is not implemented");
}
/* ************************************************************************* */
double SymbolicConditional::evaluate(const HybridValues& c) const {
throw std::runtime_error("SymbolicConditional::evaluate is not implemented");
}
} // namespace gtsam

20
gtsam/symbolic/SymbolicConditional.h
View File

@ -17,10 +17,10 @@
#pragma once
#include <gtsam/symbolic/SymbolicFactor.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/types.h>
#include <gtsam/inference/Conditional-inst.h>
#include <gtsam/symbolic/SymbolicFactor.h>
namespace gtsam {
@ -95,13 +95,10 @@ namespace gtsam {
return FromIteratorsShared(keys.begin(), keys.end(), nrFrontals);
}
~SymbolicConditional() override {}
/// Copy this object as its actual derived type.
SymbolicFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
/// @}
/// @name Testable
/// @{
@ -114,6 +111,19 @@ namespace gtsam {
bool equals(const This& c, double tol = 1e-9) const;
/// @}
/// @name HybridValues methods.
/// @{
/// logProbability throws exception, symbolic.
double logProbability(const HybridValues& x) const override;
/// evaluate throws exception, symbolic.
double evaluate(const HybridValues& x) const override;
using Conditional::operator(); // Expose evaluate(const HybridValues&) method..
using SymbolicFactor::error; // Expose error(const HybridValues&) method..
/// @}
private:
/** Serialization function */

5
gtsam/symbolic/SymbolicFactor.cpp
View File

@ -26,6 +26,11 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
double SymbolicFactor::error(const HybridValues& c) const {
throw std::runtime_error("SymbolicFactor::error is not implemented");
}
/* ************************************************************************* */
std::pair<boost::shared_ptr<SymbolicConditional>, boost::shared_ptr<SymbolicFactor> >
EliminateSymbolic(const SymbolicFactorGraph& factors, const Ordering& keys)

9
gtsam/symbolic/SymbolicFactor.h
View File

@ -30,6 +30,7 @@ namespace gtsam {
// Forward declarations
class SymbolicConditional;
class HybridValues;
class Ordering;
/** SymbolicFactor represents a symbolic factor that specifies graph topology but is not
@ -46,7 +47,7 @@ namespace gtsam {
/** Overriding the shared_ptr typedef */
typedef boost::shared_ptr<This> shared_ptr;
/// @name Standard Interface
/// @name Standard Constructors
/// @{
/** Default constructor for I/O */
@ -106,10 +107,9 @@ namespace gtsam {
}
/// @}
/// @name Advanced Constructors
/// @{
public:
/** Constructor from a collection of keys */
template<typename KEYITERATOR>
static SymbolicFactor FromIterators(KEYITERATOR beginKey, KEYITERATOR endKey) {
@ -143,6 +143,9 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/// The `error` method throws an exception.
double error(const HybridValues& c) const override;
/** Eliminate the variables in \c keys, in the order specified in \c keys, returning a
* conditional and marginal. */
std::pair<boost::shared_ptr<SymbolicConditional>, boost::shared_ptr<SymbolicFactor> >

16
python/gtsam/tests/test_DiscreteBayesNet.py
View File

@ -11,13 +11,15 @@ Author: Frank Dellaert
# pylint: disable=no-name-in-module, invalid-name
import math
import textwrap
import unittest
from gtsam.utils.test_case import GtsamTestCase
import gtsam
from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteDistribution,
DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering)
from gtsam.utils.test_case import GtsamTestCase
# Some keys:
Asia = (0, 2)
@ -111,7 +113,7 @@ class TestDiscreteBayesNet(GtsamTestCase):
self.assertEqual(len(actualSample), 8)
def test_fragment(self):
"""Test sampling and optimizing for Asia fragment."""
"""Test evaluate/sampling/optimizing for Asia fragment."""
# Create a reverse-topologically sorted fragment:
fragment = DiscreteBayesNet()
@ -125,8 +127,14 @@ class TestDiscreteBayesNet(GtsamTestCase):
given[key[0]] = 0
# Now sample from fragment:
actual = fragment.sample(given)
self.assertEqual(len(actual), 5)
values = fragment.sample(given)
self.assertEqual(len(values), 5)
for i in [0, 1, 2]:
self.assertAlmostEqual(fragment.at(i).logProbability(values),
math.log(fragment.at(i).evaluate(values)))
self.assertAlmostEqual(fragment.logProbability(values),
math.log(fragment.evaluate(values)))
def test_dot(self):
"""Check that dot works with position hints."""

19
python/gtsam/tests/test_GaussianBayesNet.py
View File

@ -10,15 +10,14 @@ Author: Frank Dellaert
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
from __future__ import print_function
import unittest
import gtsam
import numpy as np
from gtsam import GaussianBayesNet, GaussianConditional
from gtsam.utils.test_case import GtsamTestCase
import gtsam
from gtsam import GaussianBayesNet, GaussianConditional
# some keys
_x_ = 11
_y_ = 22
@ -45,6 +44,18 @@ class TestGaussianBayesNet(GtsamTestCase):
np.testing.assert_equal(R, R1)
np.testing.assert_equal(d, d1)
def test_evaluate(self):
"""Test evaluate method"""
bayesNet = smallBayesNet()
values = gtsam.VectorValues()
values.insert(_x_, np.array([9.0]))
values.insert(_y_, np.array([5.0]))
for i in [0, 1]:
self.assertAlmostEqual(bayesNet.at(i).logProbability(values),
np.log(bayesNet.at(i).evaluate(values)))
self.assertAlmostEqual(bayesNet.logProbability(values),
np.log(bayesNet.evaluate(values)))
def test_sample(self):
"""Test sample method"""
bayesNet = smallBayesNet()

15
python/gtsam/tests/test_HybridBayesNet.py
View File

@ -10,14 +10,15 @@ Author: Frank Dellaert
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
import math
import unittest
import numpy as np
from gtsam.symbol_shorthand import A, X
from gtsam.utils.test_case import GtsamTestCase
from gtsam import (DiscreteKeys, GaussianMixture, DiscreteConditional, GaussianConditional, GaussianMixture,
HybridBayesNet, HybridValues, noiseModel)
from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional,
GaussianMixture, HybridBayesNet, HybridValues, noiseModel)
class TestHybridBayesNet(GtsamTestCase):
@ -30,7 +31,7 @@ class TestHybridBayesNet(GtsamTestCase):
# Create the continuous conditional
I_1x1 = np.eye(1)
gc = GaussianConditional.FromMeanAndStddev(X(0), 2 * I_1x1, X(1), [-4],
conditional = GaussianConditional.FromMeanAndStddev(X(0), 2 * I_1x1, X(1), [-4],
5.0)
# Create the noise models
@ -45,7 +46,7 @@ class TestHybridBayesNet(GtsamTestCase):
# Create hybrid Bayes net.
bayesNet = HybridBayesNet()
bayesNet.push_back(gc)
bayesNet.push_back(conditional)
bayesNet.push_back(GaussianMixture(
[X(1)], [], discrete_keys, [conditional0, conditional1]))
bayesNet.push_back(DiscreteConditional(Asia, "99/1"))
@ -56,13 +57,17 @@ class TestHybridBayesNet(GtsamTestCase):
values.insert(X(0), [-6])
values.insert(X(1), [1])
conditionalProbability = gc.evaluate(values.continuous())
conditionalProbability = conditional.evaluate(values.continuous())
mixtureProbability = conditional0.evaluate(values.continuous())
self.assertAlmostEqual(conditionalProbability * mixtureProbability *
0.99,
bayesNet.evaluate(values),
places=5)
# Check logProbability
self.assertAlmostEqual(bayesNet.logProbability(values),
math.log(bayesNet.evaluate(values)))
if __name__ == "__main__":
unittest.main()

17
tests/testDoglegOptimizer.cpp
View File

@ -126,17 +126,22 @@ TEST(DoglegOptimizer, Iterate) {
double Delta = 1.0;
for(size_t it=0; it<10; ++it) {
GaussianBayesNet gbn = *fg.linearize(config)->eliminateSequential();
auto linearized = fg.linearize(config);
// Iterate assumes that linear error = nonlinear error at the linearization point, and this should be true
double nonlinearError = fg.error(config);
double linearError = GaussianFactorGraph(gbn).error(config.zeroVectors());
double linearError = linearized->error(config.zeroVectors());
DOUBLES_EQUAL(nonlinearError, linearError, 1e-5);
// cout << "it " << it << ", Delta = " << Delta << ", error = " << fg->error(*config) << endl;
VectorValues dx_u = gbn.optimizeGradientSearch();
VectorValues dx_n = gbn.optimize();
DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, dx_u, dx_n, gbn, fg, config, fg.error(config));
auto gbn = linearized->eliminateSequential();
VectorValues dx_u = gbn->optimizeGradientSearch();
VectorValues dx_n = gbn->optimize();
DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(
Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, dx_u, dx_n, *gbn, fg,
config, fg.error(config));
Delta = result.delta;
EXPECT(result.f_error < fg.error(config)); // Check that error decreases
Values newConfig(config.retract(result.dx_d));
config = newConfig;
DOUBLES_EQUAL(fg.error(config), result.f_error, 1e-5); // Check that error is correctly filled in