remove changes so we can break up PR into smaller ones
Varun Agrawal
parent
6d9fc8e5f2
commit
fd2062b207
|
|
@ -24,7 +24,6 @@
|
|||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridValues.h>
|
||||
#include <gtsam/inference/Conditional-inst.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -93,35 +92,6 @@ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const {
|
|||
return {conditionals_, wrap};
|
||||
}
|
||||
|
||||
/*
|
||||
*******************************************************************************/
|
||||
GaussianBayesNetTree GaussianMixture::add(
|
||||
const GaussianBayesNetTree &sum) const {
|
||||
using Y = GaussianBayesNet;
|
||||
auto add = [](const Y &graph1, const Y &graph2) {
|
||||
auto result = graph1;
|
||||
if (graph2.size() == 0) {
|
||||
return GaussianBayesNet();
|
||||
}
|
||||
result.push_back(graph2);
|
||||
return result;
|
||||
};
|
||||
const auto tree = asGaussianBayesNetTree();
|
||||
return sum.empty() ? tree : sum.apply(tree, add);
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
GaussianBayesNetTree GaussianMixture::asGaussianBayesNetTree() const {
|
||||
auto wrap = [](const GaussianConditional::shared_ptr &gc) {
|
||||
if (gc) {
|
||||
return GaussianBayesNet{gc};
|
||||
} else {
|
||||
return GaussianBayesNet();
|
||||
}
|
||||
};
|
||||
return {conditionals_, wrap};
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
size_t GaussianMixture::nrComponents() const {
|
||||
size_t total = 0;
|
||||
|
|
@ -348,15 +318,8 @@ AlgebraicDecisionTree<Key> GaussianMixture::logProbability(
|
|||
AlgebraicDecisionTree<Key> GaussianMixture::errorTree(
|
||||
const VectorValues &continuousValues) const {
|
||||
auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) {
|
||||
// Check if valid pointer
|
||||
if (conditional) {
|
||||
return conditional->error(continuousValues) + //
|
||||
logConstant_ - conditional->logNormalizationConstant();
|
||||
} else {
|
||||
// If not valid, pointer, it means this conditional was pruned,
|
||||
// so we return maximum error.
|
||||
return std::numeric_limits<double>::max();
|
||||
}
|
||||
return conditional->error(continuousValues) + //
|
||||
logConstant_ - conditional->logNormalizationConstant();
|
||||
};
|
||||
DecisionTree<Key, double> error_tree(conditionals_, errorFunc);
|
||||
return error_tree;
|
||||
|
|
@ -364,32 +327,10 @@ AlgebraicDecisionTree<Key> GaussianMixture::errorTree(
|
|||
|
||||
/* *******************************************************************************/
|
||||
double GaussianMixture::error(const HybridValues &values) const {
|
||||
// Check if discrete keys in discrete assignment are
|
||||
// present in the GaussianMixture
|
||||
KeyVector dKeys = this->discreteKeys_.indices();
|
||||
bool valid_assignment = false;
|
||||
for (auto &&kv : values.discrete()) {
|
||||
if (std::find(dKeys.begin(), dKeys.end(), kv.first) != dKeys.end()) {
|
||||
valid_assignment = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// The discrete assignment is not valid so we return 0.0 erorr.
|
||||
if (!valid_assignment) {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
// Directly index to get the conditional, no need to build the whole tree.
|
||||
auto conditional = conditionals_(values.discrete());
|
||||
if (conditional) {
|
||||
return conditional->error(values.continuous()) + //
|
||||
logConstant_ - conditional->logNormalizationConstant();
|
||||
} else {
|
||||
// If not valid, pointer, it means this conditional was pruned,
|
||||
// so we return maximum error.
|
||||
return std::numeric_limits<double>::max();
|
||||
}
|
||||
return conditional->error(values.continuous()) + //
|
||||
logConstant_ - conditional->logNormalizationConstant();
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
|
|
|
|||
|
|
@ -72,12 +72,6 @@ class GTSAM_EXPORT GaussianMixture
|
|||
*/
|
||||
GaussianFactorGraphTree asGaussianFactorGraphTree() const;
|
||||
|
||||
/**
|
||||
* @brief Convert a DecisionTree of conditionals into
|
||||
* a DecisionTree of Gaussian Bayes nets.
|
||||
*/
|
||||
GaussianBayesNetTree asGaussianBayesNetTree() const;
|
||||
|
||||
/**
|
||||
* @brief Helper function to get the pruner functor.
|
||||
*
|
||||
|
|
@ -221,7 +215,8 @@ class GTSAM_EXPORT GaussianMixture
|
|||
* @return AlgebraicDecisionTree<Key> A decision tree on the discrete keys
|
||||
* only, with the leaf values as the error for each assignment.
|
||||
*/
|
||||
AlgebraicDecisionTree<Key> errorTree(const VectorValues &continuousValues) const;
|
||||
AlgebraicDecisionTree<Key> errorTree(
|
||||
const VectorValues &continuousValues) const;
|
||||
|
||||
/**
|
||||
* @brief Compute the logProbability of this Gaussian Mixture.
|
||||
|
|
@ -255,15 +250,6 @@ class GTSAM_EXPORT GaussianMixture
|
|||
* @return GaussianFactorGraphTree
|
||||
*/
|
||||
GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const;
|
||||
|
||||
/**
|
||||
* @brief Merge the Gaussian Bayes Nets in `this` and `sum` while
|
||||
* maintaining the decision tree structure.
|
||||
*
|
||||
* @param sum Decision Tree of Gaussian Bayes Nets
|
||||
* @return GaussianBayesNetTree
|
||||
*/
|
||||
GaussianBayesNetTree add(const GaussianBayesNetTree &sum) const;
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -28,86 +28,11 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* @brief Helper function to correct the [A|b] matrices in the factor components
|
||||
* with the normalizer values.
|
||||
* This is done by storing the normalizer value in
|
||||
* the `b` vector as an additional row.
|
||||
*
|
||||
* @param factors DecisionTree of GaussianFactor shared pointers.
|
||||
* @param varyingNormalizers Flag indicating the normalizers are different for
|
||||
* each component.
|
||||
* @return GaussianMixtureFactor::Factors
|
||||
*/
|
||||
GaussianMixtureFactor::Factors correct(
|
||||
const GaussianMixtureFactor::Factors &factors, bool varyingNormalizers) {
|
||||
if (!varyingNormalizers) {
|
||||
return factors;
|
||||
}
|
||||
|
||||
// First compute all the sqrt(|2 pi Sigma|) terms
|
||||
auto computeNormalizers = [](const GaussianMixtureFactor::sharedFactor &gf) {
|
||||
auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
// If we have, say, a Hessian factor, then no need to do anything
|
||||
if (!jf) return 0.0;
|
||||
|
||||
auto model = jf->get_model();
|
||||
// If there is no noise model, there is nothing to do.
|
||||
if (!model) {
|
||||
return 0.0;
|
||||
}
|
||||
// Since noise models are Gaussian, we can get the logDeterminant using the
|
||||
// same trick as in GaussianConditional
|
||||
double logDetR =
|
||||
model->R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
|
||||
double logDeterminantSigma = -2.0 * logDetR;
|
||||
|
||||
size_t n = model->dim();
|
||||
constexpr double log2pi = 1.8378770664093454835606594728112;
|
||||
return n * log2pi + logDeterminantSigma;
|
||||
};
|
||||
|
||||
AlgebraicDecisionTree<Key> log_normalizers =
|
||||
DecisionTree<Key, double>(factors, computeNormalizers);
|
||||
|
||||
// Find the minimum value so we can "proselytize" to positive values.
|
||||
// Done because we can't have sqrt of negative numbers.
|
||||
double min_log_normalizer = log_normalizers.min();
|
||||
log_normalizers = log_normalizers.apply(
|
||||
[&min_log_normalizer](double n) { return n - min_log_normalizer; });
|
||||
|
||||
// Finally, update the [A|b] matrices.
|
||||
auto update = [&log_normalizers](
|
||||
const Assignment<Key> &assignment,
|
||||
const GaussianMixtureFactor::sharedFactor &gf) {
|
||||
auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
if (!jf) return gf;
|
||||
// If there is no noise model, there is nothing to do.
|
||||
if (!jf->get_model()) return gf;
|
||||
// If the log_normalizer is 0, do nothing
|
||||
if (log_normalizers(assignment) == 0.0) return gf;
|
||||
|
||||
GaussianFactorGraph gfg;
|
||||
gfg.push_back(jf);
|
||||
|
||||
Vector c(1);
|
||||
c << std::sqrt(log_normalizers(assignment));
|
||||
auto constantFactor = std::make_shared<JacobianFactor>(c);
|
||||
|
||||
gfg.push_back(constantFactor);
|
||||
return std::dynamic_pointer_cast<GaussianFactor>(
|
||||
std::make_shared<JacobianFactor>(gfg));
|
||||
};
|
||||
return factors.apply(update);
|
||||
}
|
||||
|
||||
/* *******************************************************************************/
|
||||
GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
|
||||
const DiscreteKeys &discreteKeys,
|
||||
const Factors &factors,
|
||||
bool varyingNormalizers)
|
||||
: Base(continuousKeys, discreteKeys),
|
||||
factors_(correct(factors, varyingNormalizers)) {}
|
||||
const Factors &factors)
|
||||
: Base(continuousKeys, discreteKeys), factors_(factors) {}
|
||||
|
||||
/* *******************************************************************************/
|
||||
bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
|
||||
|
|
@ -129,9 +54,7 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
|
|||
/* *******************************************************************************/
|
||||
void GaussianMixtureFactor::print(const std::string &s,
|
||||
const KeyFormatter &formatter) const {
|
||||
std::cout << (s.empty() ? "" : s + "\n");
|
||||
std::cout << "GaussianMixtureFactor" << std::endl;
|
||||
HybridFactor::print("", formatter);
|
||||
HybridFactor::print(s, formatter);
|
||||
std::cout << "{\n";
|
||||
if (factors_.empty()) {
|
||||
std::cout << " empty" << std::endl;
|
||||
|
|
|
|||
|
|
@ -82,13 +82,10 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
|
|||
* their cardinalities.
|
||||
* @param factors The decision tree of Gaussian factors stored as the mixture
|
||||
* density.
|
||||
* @param varyingNormalizers Flag indicating factor components have varying
|
||||
* normalizer values.
|
||||
*/
|
||||
GaussianMixtureFactor(const KeyVector &continuousKeys,
|
||||
const DiscreteKeys &discreteKeys,
|
||||
const Factors &factors,
|
||||
bool varyingNormalizers = false);
|
||||
const Factors &factors);
|
||||
|
||||
/**
|
||||
* @brief Construct a new GaussianMixtureFactor object using a vector of
|
||||
|
|
@ -97,16 +94,12 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
|
|||
* @param continuousKeys Vector of keys for continuous factors.
|
||||
* @param discreteKeys Vector of discrete keys.
|
||||
* @param factors Vector of gaussian factor shared pointers.
|
||||
* @param varyingNormalizers Flag indicating factor components have varying
|
||||
* normalizer values.
|
||||
*/
|
||||
GaussianMixtureFactor(const KeyVector &continuousKeys,
|
||||
const DiscreteKeys &discreteKeys,
|
||||
const std::vector<sharedFactor> &factors,
|
||||
bool varyingNormalizers = false)
|
||||
const std::vector<sharedFactor> &factors)
|
||||
: GaussianMixtureFactor(continuousKeys, discreteKeys,
|
||||
Factors(discreteKeys, factors),
|
||||
varyingNormalizers) {}
|
||||
Factors(discreteKeys, factors)) {}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
|
|
@ -114,8 +107,9 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
|
|||
|
||||
bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
|
||||
|
||||
void print(const std::string &s = "", const KeyFormatter &formatter =
|
||||
DefaultKeyFormatter) const override;
|
||||
void print(
|
||||
const std::string &s = "GaussianMixtureFactor\n",
|
||||
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
|
||||
|
||||
/// @}
|
||||
/// @name Standard API
|
||||
|
|
|
|||
|
|
@ -220,16 +220,16 @@ GaussianBayesNet HybridBayesNet::choose(
|
|||
/* ************************************************************************* */
|
||||
HybridValues HybridBayesNet::optimize() const {
|
||||
// Collect all the discrete factors to compute MPE
|
||||
DiscreteFactorGraph discrete_fg;
|
||||
DiscreteBayesNet discrete_bn;
|
||||
|
||||
for (auto &&conditional : *this) {
|
||||
if (conditional->isDiscrete()) {
|
||||
discrete_fg.push_back(conditional->asDiscrete());
|
||||
discrete_bn.push_back(conditional->asDiscrete());
|
||||
}
|
||||
}
|
||||
|
||||
// Solve for the MPE
|
||||
DiscreteValues mpe = discrete_fg.optimize();
|
||||
DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).optimize();
|
||||
|
||||
// Given the MPE, compute the optimal continuous values.
|
||||
return HybridValues(optimize(mpe), mpe);
|
||||
|
|
|
|||
|
|
@ -13,7 +13,6 @@
|
|||
* @file HybridFactor.h
|
||||
* @date Mar 11, 2022
|
||||
* @author Fan Jiang
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -34,8 +33,6 @@ class HybridValues;
|
|||
|
||||
/// Alias for DecisionTree of GaussianFactorGraphs
|
||||
using GaussianFactorGraphTree = DecisionTree<Key, GaussianFactorGraph>;
|
||||
/// Alias for DecisionTree of GaussianBayesNets
|
||||
using GaussianBayesNetTree = DecisionTree<Key, GaussianBayesNet>;
|
||||
|
||||
KeyVector CollectKeys(const KeyVector &continuousKeys,
|
||||
const DiscreteKeys &discreteKeys);
|
||||
|
|
|
|||
|
|
@ -279,37 +279,21 @@ GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) {
|
|||
using Result = std::pair<std::shared_ptr<GaussianConditional>,
|
||||
GaussianMixtureFactor::sharedFactor>;
|
||||
|
||||
/**
|
||||
* Compute the probability q(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m)
|
||||
* from the residual error at the mean μ.
|
||||
* The residual error contains no keys, and only
|
||||
* depends on the discrete separator if present.
|
||||
*/
|
||||
// Integrate the probability mass in the last continuous conditional using
|
||||
// the unnormalized probability q(μ;m) = exp(-error(μ;m)) at the mean.
|
||||
// discrete_probability = exp(-error(μ;m)) * sqrt(det(2π Σ_m))
|
||||
static std::shared_ptr<Factor> createDiscreteFactor(
|
||||
const DecisionTree<Key, Result> &eliminationResults,
|
||||
const DiscreteKeys &discreteSeparator) {
|
||||
auto logProbability = [&](const Result &pair) -> double {
|
||||
auto probability = [&](const Result &pair) -> double {
|
||||
const auto &[conditional, factor] = pair;
|
||||
static const VectorValues kEmpty;
|
||||
// If the factor is not null, it has no keys, just contains the residual.
|
||||
if (!factor) return 1.0; // TODO(dellaert): not loving this.
|
||||
|
||||
// Logspace version of:
|
||||
// exp(-factor->error(kEmpty)) / conditional->normalizationConstant();
|
||||
// We take negative of the logNormalizationConstant `log(1/k)`
|
||||
// to get `log(k)`.
|
||||
return -factor->error(kEmpty) + (-conditional->logNormalizationConstant());
|
||||
return exp(-factor->error(kEmpty)) / conditional->normalizationConstant();
|
||||
};
|
||||
|
||||
AlgebraicDecisionTree<Key> logProbabilities(
|
||||
DecisionTree<Key, double>(eliminationResults, logProbability));
|
||||
|
||||
// Perform normalization
|
||||
double max_log = logProbabilities.max();
|
||||
AlgebraicDecisionTree probabilities = DecisionTree<Key, double>(
|
||||
logProbabilities,
|
||||
[&max_log](const double x) { return exp(x - max_log); });
|
||||
probabilities = probabilities.normalize(probabilities.sum());
|
||||
DecisionTree<Key, double> probabilities(eliminationResults, probability);
|
||||
|
||||
return std::make_shared<DecisionTreeFactor>(discreteSeparator, probabilities);
|
||||
}
|
||||
|
|
@ -370,12 +354,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
|
|||
// Perform elimination!
|
||||
DecisionTree<Key, Result> eliminationResults(factorGraphTree, eliminate);
|
||||
|
||||
// Create the GaussianMixture from the conditionals
|
||||
GaussianMixture::Conditionals conditionals(
|
||||
eliminationResults, [](const Result &pair) { return pair.first; });
|
||||
auto gaussianMixture = std::make_shared<GaussianMixture>(
|
||||
frontalKeys, continuousSeparator, discreteSeparator, conditionals);
|
||||
|
||||
// If there are no more continuous parents we create a DiscreteFactor with the
|
||||
// error for each discrete choice. Otherwise, create a GaussianMixtureFactor
|
||||
// on the separator, taking care to correct for conditional constants.
|
||||
|
|
@ -385,6 +363,12 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
|
|||
: createGaussianMixtureFactor(eliminationResults, continuousSeparator,
|
||||
discreteSeparator);
|
||||
|
||||
// Create the GaussianMixture from the conditionals
|
||||
GaussianMixture::Conditionals conditionals(
|
||||
eliminationResults, [](const Result &pair) { return pair.first; });
|
||||
auto gaussianMixture = std::make_shared<GaussianMixture>(
|
||||
frontalKeys, continuousSeparator, discreteSeparator, conditionals);
|
||||
|
||||
return {std::make_shared<HybridConditional>(gaussianMixture), newFactor};
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -22,13 +22,9 @@
|
|||
#include <gtsam/discrete/DiscreteValues.h>
|
||||
#include <gtsam/hybrid/GaussianMixture.h>
|
||||
#include <gtsam/hybrid/GaussianMixtureFactor.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridValues.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
// Include for test suite
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -60,6 +56,7 @@ TEST(GaussianMixtureFactor, Sum) {
|
|||
auto b = Matrix::Zero(2, 1);
|
||||
Vector2 sigmas;
|
||||
sigmas << 1, 2;
|
||||
auto model = noiseModel::Diagonal::Sigmas(sigmas, true);
|
||||
|
||||
auto f10 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
|
||||
auto f11 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
|
||||
|
|
@ -109,8 +106,7 @@ TEST(GaussianMixtureFactor, Printing) {
|
|||
GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
|
||||
|
||||
std::string expected =
|
||||
R"(GaussianMixtureFactor
|
||||
Hybrid [x1 x2; 1]{
|
||||
R"(Hybrid [x1 x2; 1]{
|
||||
Choice(1)
|
||||
0 Leaf :
|
||||
A[x1] = [
|
||||
|
|
@ -182,8 +178,7 @@ TEST(GaussianMixtureFactor, Error) {
|
|||
continuousValues.insert(X(2), Vector2(1, 1));
|
||||
|
||||
// error should return a tree of errors, with nodes for each discrete value.
|
||||
AlgebraicDecisionTree<Key> error_tree =
|
||||
mixtureFactor.errorTree(continuousValues);
|
||||
AlgebraicDecisionTree<Key> error_tree = mixtureFactor.errorTree(continuousValues);
|
||||
|
||||
std::vector<DiscreteKey> discrete_keys = {m1};
|
||||
// Error values for regression test
|
||||
|
|
@ -196,240 +191,8 @@ TEST(GaussianMixtureFactor, Error) {
|
|||
DiscreteValues discreteValues;
|
||||
discreteValues[m1.first] = 1;
|
||||
EXPECT_DOUBLES_EQUAL(
|
||||
4.0, mixtureFactor.error({continuousValues, discreteValues}), 1e-9);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test components with differing means
|
||||
TEST(GaussianMixtureFactor, DifferentMeans) {
|
||||
DiscreteKey m1(M(1), 2), m2(M(2), 2);
|
||||
|
||||
Values values;
|
||||
double x1 = 0.0, x2 = 1.75, x3 = 2.60;
|
||||
values.insert(X(1), x1);
|
||||
values.insert(X(2), x2);
|
||||
values.insert(X(3), x3);
|
||||
|
||||
auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0);
|
||||
auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0);
|
||||
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0);
|
||||
|
||||
auto f0 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 0.0, model0)
|
||||
->linearize(values);
|
||||
auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 2.0, model1)
|
||||
->linearize(values);
|
||||
std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
|
||||
|
||||
GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors, true);
|
||||
HybridGaussianFactorGraph hfg;
|
||||
hfg.push_back(mixtureFactor);
|
||||
|
||||
f0 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 0.0, model0)
|
||||
->linearize(values);
|
||||
f1 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 2.0, model1)
|
||||
->linearize(values);
|
||||
std::vector<GaussianFactor::shared_ptr> factors23{f0, f1};
|
||||
hfg.push_back(GaussianMixtureFactor({X(2), X(3)}, {m2}, factors23, true));
|
||||
|
||||
auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
|
||||
hfg.push_back(prior);
|
||||
|
||||
hfg.push_back(PriorFactor<double>(X(2), 2.0, prior_noise).linearize(values));
|
||||
|
||||
auto bn = hfg.eliminateSequential();
|
||||
HybridValues actual = bn->optimize();
|
||||
|
||||
HybridValues expected(
|
||||
VectorValues{
|
||||
{X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}},
|
||||
DiscreteValues{{M(1), 1}, {M(2), 0}});
|
||||
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
|
||||
{
|
||||
DiscreteValues dv{{M(1), 0}, {M(2), 0}};
|
||||
VectorValues cont = bn->optimize(dv);
|
||||
double error = bn->error(HybridValues(cont, dv));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
|
||||
}
|
||||
{
|
||||
DiscreteValues dv{{M(1), 0}, {M(2), 1}};
|
||||
VectorValues cont = bn->optimize(dv);
|
||||
double error = bn->error(HybridValues(cont, dv));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
|
||||
}
|
||||
{
|
||||
DiscreteValues dv{{M(1), 1}, {M(2), 0}};
|
||||
VectorValues cont = bn->optimize(dv);
|
||||
double error = bn->error(HybridValues(cont, dv));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
|
||||
}
|
||||
{
|
||||
DiscreteValues dv{{M(1), 1}, {M(2), 1}};
|
||||
VectorValues cont = bn->optimize(dv);
|
||||
double error = bn->error(HybridValues(cont, dv));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* @brief Test components with differing covariances.
|
||||
* The factor graph is
|
||||
* *-X1-*-X2
|
||||
* |
|
||||
* M1
|
||||
*/
|
||||
TEST(GaussianMixtureFactor, DifferentCovariances) {
|
||||
DiscreteKey m1(M(1), 2);
|
||||
|
||||
Values values;
|
||||
double x1 = 1.0, x2 = 1.0;
|
||||
values.insert(X(1), x1);
|
||||
values.insert(X(2), x2);
|
||||
|
||||
double between = 0.0;
|
||||
|
||||
auto model0 = noiseModel::Isotropic::Sigma(1, 1e2);
|
||||
auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2);
|
||||
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
|
||||
|
||||
auto f0 =
|
||||
std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model0);
|
||||
auto f1 =
|
||||
std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model1);
|
||||
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
|
||||
|
||||
// Create via toFactorGraph
|
||||
using symbol_shorthand::Z;
|
||||
Matrix H0_1, H0_2, H1_1, H1_2;
|
||||
Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2);
|
||||
std::vector<std::pair<Key, Matrix>> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/},
|
||||
//
|
||||
{X(1), H0_1 /*Sp1*/},
|
||||
{X(2), H0_2 /*Tp2*/}};
|
||||
|
||||
Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2);
|
||||
std::vector<std::pair<Key, Matrix>> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/},
|
||||
//
|
||||
{X(1), H1_1 /*Sp1*/},
|
||||
{X(2), H1_2 /*Tp2*/}};
|
||||
gtsam::GaussianMixtureFactor gmf(
|
||||
{X(1), X(2)}, {m1},
|
||||
{std::make_shared<JacobianFactor>(X(1), H0_1, X(2), H0_2, -d0, model0),
|
||||
std::make_shared<JacobianFactor>(X(1), H1_1, X(2), H1_2, -d1, model1)},
|
||||
true);
|
||||
|
||||
// Create FG with single GaussianMixtureFactor
|
||||
HybridGaussianFactorGraph mixture_fg;
|
||||
mixture_fg.add(gmf);
|
||||
|
||||
// Linearized prior factor on X1
|
||||
auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
|
||||
mixture_fg.push_back(prior);
|
||||
|
||||
auto hbn = mixture_fg.eliminateSequential();
|
||||
// hbn->print();
|
||||
|
||||
VectorValues cv;
|
||||
cv.insert(X(1), Vector1(0.0));
|
||||
cv.insert(X(2), Vector1(0.0));
|
||||
|
||||
// Check that the error values at the MLE point μ.
|
||||
AlgebraicDecisionTree<Key> errorTree = hbn->errorTree(cv);
|
||||
|
||||
DiscreteValues dv0{{M(1), 0}};
|
||||
DiscreteValues dv1{{M(1), 1}};
|
||||
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(9.90348755254, errorTree(dv0), 1e-9);
|
||||
EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9);
|
||||
|
||||
DiscreteConditional expected_m1(m1, "0.5/0.5");
|
||||
DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete());
|
||||
|
||||
EXPECT(assert_equal(expected_m1, actual_m1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* @brief Test components with differing covariances
|
||||
* but with a Bayes net P(Z|X, M) converted to a FG.
|
||||
*/
|
||||
TEST(GaussianMixtureFactor, DifferentCovariances2) {
|
||||
DiscreteKey m1(M(1), 2);
|
||||
|
||||
Values values;
|
||||
double x1 = 1.0, x2 = 1.0;
|
||||
values.insert(X(1), x1);
|
||||
values.insert(X(2), x2);
|
||||
|
||||
double between = 0.0;
|
||||
|
||||
auto model0 = noiseModel::Isotropic::Sigma(1, 1e2);
|
||||
auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2);
|
||||
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
|
||||
|
||||
auto f0 =
|
||||
std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model0);
|
||||
auto f1 =
|
||||
std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model1);
|
||||
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
|
||||
|
||||
// Create via toFactorGraph
|
||||
using symbol_shorthand::Z;
|
||||
Matrix H0_1, H0_2, H1_1, H1_2;
|
||||
Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2);
|
||||
std::vector<std::pair<Key, Matrix>> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/},
|
||||
//
|
||||
{X(1), H0_1 /*Sp1*/},
|
||||
{X(2), H0_2 /*Tp2*/}};
|
||||
|
||||
Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2);
|
||||
std::vector<std::pair<Key, Matrix>> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/},
|
||||
//
|
||||
{X(1), H1_1 /*Sp1*/},
|
||||
{X(2), H1_2 /*Tp2*/}};
|
||||
auto gm = new gtsam::GaussianMixture(
|
||||
{Z(1)}, {X(1), X(2)}, {m1},
|
||||
{std::make_shared<GaussianConditional>(terms0, 1, -d0, model0),
|
||||
std::make_shared<GaussianConditional>(terms1, 1, -d1, model1)});
|
||||
gtsam::HybridBayesNet bn;
|
||||
bn.emplace_back(gm);
|
||||
|
||||
gtsam::VectorValues measurements;
|
||||
measurements.insert(Z(1), gtsam::Z_1x1);
|
||||
// Create FG with single GaussianMixtureFactor
|
||||
HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements);
|
||||
|
||||
// Linearized prior factor on X1
|
||||
auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
|
||||
mixture_fg.push_back(prior);
|
||||
|
||||
auto hbn = mixture_fg.eliminateSequential();
|
||||
|
||||
VectorValues cv;
|
||||
cv.insert(X(1), Vector1(0.0));
|
||||
cv.insert(X(2), Vector1(0.0));
|
||||
|
||||
// Check that the error values at the MLE point μ.
|
||||
AlgebraicDecisionTree<Key> errorTree = hbn->errorTree(cv);
|
||||
|
||||
DiscreteValues dv0{{M(1), 0}};
|
||||
DiscreteValues dv1{{M(1), 1}};
|
||||
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(9.90348755254, errorTree(dv0), 1e-9);
|
||||
EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9);
|
||||
|
||||
DiscreteConditional expected_m1(m1, "0.5/0.5");
|
||||
DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete());
|
||||
|
||||
EXPECT(assert_equal(expected_m1, actual_m1));
|
||||
4.0, mixtureFactor.error({continuousValues, discreteValues}),
|
||||
1e-9);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -510,7 +510,6 @@ factor 0:
|
|||
b = [ -10 ]
|
||||
No noise model
|
||||
factor 1:
|
||||
GaussianMixtureFactor
|
||||
Hybrid [x0 x1; m0]{
|
||||
Choice(m0)
|
||||
0 Leaf :
|
||||
|
|
@ -535,7 +534,6 @@ Hybrid [x0 x1; m0]{
|
|||
|
||||
}
|
||||
factor 2:
|
||||
GaussianMixtureFactor
|
||||
Hybrid [x1 x2; m1]{
|
||||
Choice(m1)
|
||||
0 Leaf :
|
||||
|
|
@ -677,26 +675,22 @@ factor 6: P( m1 | m0 ):
|
|||
size: 3
|
||||
conditional 0: Hybrid P( x0 | x1 m0)
|
||||
Discrete Keys = (m0, 2),
|
||||
logNormalizationConstant: 1.38862
|
||||
|
||||
Choice(m0)
|
||||
0 Leaf p(x0 | x1)
|
||||
R = [ 10.0499 ]
|
||||
S[x1] = [ -0.0995037 ]
|
||||
d = [ -9.85087 ]
|
||||
logNormalizationConstant: 1.38862
|
||||
No noise model
|
||||
|
||||
1 Leaf p(x0 | x1)
|
||||
R = [ 10.0499 ]
|
||||
S[x1] = [ -0.0995037 ]
|
||||
d = [ -9.95037 ]
|
||||
logNormalizationConstant: 1.38862
|
||||
No noise model
|
||||
|
||||
conditional 1: Hybrid P( x1 | x2 m0 m1)
|
||||
Discrete Keys = (m0, 2), (m1, 2),
|
||||
logNormalizationConstant: 1.3935
|
||||
|
||||
Choice(m1)
|
||||
0 Choice(m0)
|
||||
|
|
@ -704,14 +698,12 @@ conditional 1: Hybrid P( x1 | x2 m0 m1)
|
|||
R = [ 10.099 ]
|
||||
S[x2] = [ -0.0990196 ]
|
||||
d = [ -9.99901 ]
|
||||
logNormalizationConstant: 1.3935
|
||||
No noise model
|
||||
|
||||
0 1 Leaf p(x1 | x2)
|
||||
R = [ 10.099 ]
|
||||
S[x2] = [ -0.0990196 ]
|
||||
d = [ -9.90098 ]
|
||||
logNormalizationConstant: 1.3935
|
||||
No noise model
|
||||
|
||||
1 Choice(m0)
|
||||
|
|
@ -719,19 +711,16 @@ conditional 1: Hybrid P( x1 | x2 m0 m1)
|
|||
R = [ 10.099 ]
|
||||
S[x2] = [ -0.0990196 ]
|
||||
d = [ -10.098 ]
|
||||
logNormalizationConstant: 1.3935
|
||||
No noise model
|
||||
|
||||
1 1 Leaf p(x1 | x2)
|
||||
R = [ 10.099 ]
|
||||
S[x2] = [ -0.0990196 ]
|
||||
d = [ -10 ]
|
||||
logNormalizationConstant: 1.3935
|
||||
No noise model
|
||||
|
||||
conditional 2: Hybrid P( x2 | m0 m1)
|
||||
Discrete Keys = (m0, 2), (m1, 2),
|
||||
logNormalizationConstant: 1.38857
|
||||
|
||||
Choice(m1)
|
||||
0 Choice(m0)
|
||||
|
|
@ -740,7 +729,6 @@ conditional 2: Hybrid P( x2 | m0 m1)
|
|||
d = [ -10.1489 ]
|
||||
mean: 1 elements
|
||||
x2: -1.0099
|
||||
logNormalizationConstant: 1.38857
|
||||
No noise model
|
||||
|
||||
0 1 Leaf p(x2)
|
||||
|
|
@ -748,7 +736,6 @@ conditional 2: Hybrid P( x2 | m0 m1)
|
|||
d = [ -10.1479 ]
|
||||
mean: 1 elements
|
||||
x2: -1.0098
|
||||
logNormalizationConstant: 1.38857
|
||||
No noise model
|
||||
|
||||
1 Choice(m0)
|
||||
|
|
@ -757,7 +744,6 @@ conditional 2: Hybrid P( x2 | m0 m1)
|
|||
d = [ -10.0504 ]
|
||||
mean: 1 elements
|
||||
x2: -1.0001
|
||||
logNormalizationConstant: 1.38857
|
||||
No noise model
|
||||
|
||||
1 1 Leaf p(x2)
|
||||
|
|
@ -765,7 +751,6 @@ conditional 2: Hybrid P( x2 | m0 m1)
|
|||
d = [ -10.0494 ]
|
||||
mean: 1 elements
|
||||
x2: -1
|
||||
logNormalizationConstant: 1.38857
|
||||
No noise model
|
||||
|
||||
)";
|
||||
|
|
|
|||
|
|
@ -18,9 +18,6 @@
|
|||
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/discrete/DiscreteValues.h>
|
||||
#include <gtsam/hybrid/HybridBayesNet.h>
|
||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
|
||||
#include <gtsam/hybrid/MixtureFactor.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
|
@ -118,152 +115,6 @@ TEST(MixtureFactor, Dim) {
|
|||
EXPECT_LONGS_EQUAL(1, mixtureFactor.dim());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test components with differing means
|
||||
TEST(MixtureFactor, DifferentMeans) {
|
||||
DiscreteKey m1(M(1), 2), m2(M(2), 2);
|
||||
|
||||
Values values;
|
||||
double x1 = 0.0, x2 = 1.75, x3 = 2.60;
|
||||
values.insert(X(1), x1);
|
||||
values.insert(X(2), x2);
|
||||
values.insert(X(3), x3);
|
||||
|
||||
auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0);
|
||||
auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0);
|
||||
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0);
|
||||
|
||||
auto f0 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 0.0, model0);
|
||||
auto f1 = std::make_shared<BetweenFactor<double>>(X(1), X(2), 2.0, model1);
|
||||
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
|
||||
|
||||
MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
|
||||
HybridNonlinearFactorGraph hnfg;
|
||||
hnfg.push_back(mixtureFactor);
|
||||
|
||||
f0 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 0.0, model0);
|
||||
f1 = std::make_shared<BetweenFactor<double>>(X(2), X(3), 2.0, model1);
|
||||
std::vector<NonlinearFactor::shared_ptr> factors23{f0, f1};
|
||||
hnfg.push_back(MixtureFactor({X(2), X(3)}, {m2}, factors23));
|
||||
|
||||
auto prior = PriorFactor<double>(X(1), x1, prior_noise);
|
||||
hnfg.push_back(prior);
|
||||
|
||||
hnfg.emplace_shared<PriorFactor<double>>(X(2), 2.0, prior_noise);
|
||||
|
||||
auto hgfg = hnfg.linearize(values);
|
||||
auto bn = hgfg->eliminateSequential();
|
||||
HybridValues actual = bn->optimize();
|
||||
|
||||
HybridValues expected(
|
||||
VectorValues{
|
||||
{X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}},
|
||||
DiscreteValues{{M(1), 1}, {M(2), 0}});
|
||||
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
|
||||
{
|
||||
DiscreteValues dv{{M(1), 0}, {M(2), 0}};
|
||||
VectorValues cont = bn->optimize(dv);
|
||||
double error = bn->error(HybridValues(cont, dv));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
|
||||
}
|
||||
{
|
||||
DiscreteValues dv{{M(1), 0}, {M(2), 1}};
|
||||
VectorValues cont = bn->optimize(dv);
|
||||
double error = bn->error(HybridValues(cont, dv));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9);
|
||||
}
|
||||
{
|
||||
DiscreteValues dv{{M(1), 1}, {M(2), 0}};
|
||||
VectorValues cont = bn->optimize(dv);
|
||||
double error = bn->error(HybridValues(cont, dv));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
|
||||
}
|
||||
{
|
||||
DiscreteValues dv{{M(1), 1}, {M(2), 1}};
|
||||
VectorValues cont = bn->optimize(dv);
|
||||
double error = bn->error(HybridValues(cont, dv));
|
||||
// regression
|
||||
EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test components with differing covariances
|
||||
TEST(MixtureFactor, DifferentCovariances) {
|
||||
DiscreteKey m1(M(1), 2);
|
||||
|
||||
Values values;
|
||||
double x1 = 1.0, x2 = 1.0;
|
||||
values.insert(X(1), x1);
|
||||
values.insert(X(2), x2);
|
||||
|
||||
double between = 0.0;
|
||||
|
||||
auto model0 = noiseModel::Isotropic::Sigma(1, 1e2);
|
||||
auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2);
|
||||
auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3);
|
||||
|
||||
auto f0 =
|
||||
std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model0);
|
||||
auto f1 =
|
||||
std::make_shared<BetweenFactor<double>>(X(1), X(2), between, model1);
|
||||
std::vector<NonlinearFactor::shared_ptr> factors{f0, f1};
|
||||
|
||||
// Create via toFactorGraph
|
||||
using symbol_shorthand::Z;
|
||||
Matrix H0_1, H0_2, H1_1, H1_2;
|
||||
Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2);
|
||||
std::vector<std::pair<Key, Matrix>> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/},
|
||||
//
|
||||
{X(1), H0_1 /*Sp1*/},
|
||||
{X(2), H0_2 /*Tp2*/}};
|
||||
|
||||
Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2);
|
||||
std::vector<std::pair<Key, Matrix>> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/},
|
||||
//
|
||||
{X(1), H1_1 /*Sp1*/},
|
||||
{X(2), H1_2 /*Tp2*/}};
|
||||
auto gm = new gtsam::GaussianMixture(
|
||||
{Z(1)}, {X(1), X(2)}, {m1},
|
||||
{std::make_shared<GaussianConditional>(terms0, 1, -d0, model0),
|
||||
std::make_shared<GaussianConditional>(terms1, 1, -d1, model1)});
|
||||
gtsam::HybridBayesNet bn;
|
||||
bn.emplace_back(gm);
|
||||
|
||||
gtsam::VectorValues measurements;
|
||||
measurements.insert(Z(1), gtsam::Z_1x1);
|
||||
// Create FG with single GaussianMixtureFactor
|
||||
HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements);
|
||||
|
||||
// Linearized prior factor on X1
|
||||
auto prior = PriorFactor<double>(X(1), x1, prior_noise).linearize(values);
|
||||
mixture_fg.push_back(prior);
|
||||
|
||||
auto hbn = mixture_fg.eliminateSequential();
|
||||
|
||||
VectorValues cv;
|
||||
cv.insert(X(1), Vector1(0.0));
|
||||
cv.insert(X(2), Vector1(0.0));
|
||||
|
||||
// Check that we get different error values at the MLE point μ.
|
||||
AlgebraicDecisionTree<Key> errorTree = hbn->errorTree(cv);
|
||||
|
||||
HybridValues hv0(cv, DiscreteValues{{M(1), 0}});
|
||||
HybridValues hv1(cv, DiscreteValues{{M(1), 1}});
|
||||
|
||||
auto cond0 = hbn->at(0)->asMixture();
|
||||
auto cond1 = hbn->at(1)->asMixture();
|
||||
auto discrete_cond = hbn->at(2)->asDiscrete();
|
||||
AlgebraicDecisionTree<Key> expectedErrorTree(m1, 9.90348755254,
|
||||
0.69314718056);
|
||||
EXPECT(assert_equal(expectedErrorTree, errorTree));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -243,25 +243,5 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double GaussianBayesNet::logNormalizationConstant() const {
|
||||
/*
|
||||
normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
|
||||
logConstant = -0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
|
||||
|
||||
log det(Sigma)) = -2.0 * logDeterminant()
|
||||
thus, logConstant = -0.5*n*log(2*pi) + logDeterminant()
|
||||
|
||||
BayesNet logConstant = sum(-0.5*n_i*log(2*pi) + logDeterminant_i())
|
||||
= sum(-0.5*n_i*log(2*pi)) + sum(logDeterminant_i())
|
||||
= sum(-0.5*n_i*log(2*pi)) + bn->logDeterminant()
|
||||
*/
|
||||
double logNormConst = 0.0;
|
||||
for (const sharedConditional& cg : *this) {
|
||||
logNormConst += cg->logNormalizationConstant();
|
||||
}
|
||||
return logNormConst;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -82,12 +82,6 @@ namespace gtsam {
|
|||
/** Check equality */
|
||||
bool equals(const This& bn, double tol = 1e-9) const;
|
||||
|
||||
/// Check exact equality.
|
||||
friend bool operator==(const GaussianBayesNet& lhs,
|
||||
const GaussianBayesNet& rhs) {
|
||||
return lhs.isEqual(rhs);
|
||||
}
|
||||
|
||||
/// print graph
|
||||
void print(
|
||||
const std::string& s = "",
|
||||
|
|
@ -234,14 +228,6 @@ namespace gtsam {
|
|||
* @return The determinant */
|
||||
double logDeterminant() const;
|
||||
|
||||
/**
|
||||
* @brief Get the log of the normalization constant corresponding to the
|
||||
* joint Gaussian density represented by this Bayes net.
|
||||
*
|
||||
* @return double
|
||||
*/
|
||||
double logNormalizationConstant() const;
|
||||
|
||||
/**
|
||||
* Backsubstitute with a different RHS vector than the one stored in this BayesNet.
|
||||
* gy=inv(R*inv(Sigma))*gx
|
||||
|
|
|
|||
|
|
@ -121,7 +121,6 @@ namespace gtsam {
|
|||
const auto mean = solve({}); // solve for mean.
|
||||
mean.print(" mean", formatter);
|
||||
}
|
||||
cout << " logNormalizationConstant: " << logNormalizationConstant() << std::endl;
|
||||
if (model_)
|
||||
model_->print(" Noise model: ");
|
||||
else
|
||||
|
|
@ -185,13 +184,8 @@ namespace gtsam {
|
|||
double GaussianConditional::logNormalizationConstant() const {
|
||||
constexpr double log2pi = 1.8378770664093454835606594728112;
|
||||
size_t n = d().size();
|
||||
// Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1}
|
||||
// log det(Sigma) = -log(det(R'R)) = -2*log(det(R))
|
||||
// Hence, log det(Sigma)) = -2.0 * logDeterminant()
|
||||
// which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDeterminant())
|
||||
// = -0.5*n*log(2*pi) + (0.5*2.0 * logDeterminant())
|
||||
// = -0.5*n*log(2*pi) + logDeterminant()
|
||||
return -0.5 * n * log2pi + logDeterminant();
|
||||
// log det(Sigma)) = - 2.0 * logDeterminant()
|
||||
return - 0.5 * n * log2pi + logDeterminant();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -263,11 +263,6 @@ namespace gtsam {
|
|||
/** equals required by Testable for unit testing */
|
||||
bool equals(const VectorValues& x, double tol = 1e-9) const;
|
||||
|
||||
/// Check exact equality.
|
||||
friend bool operator==(const VectorValues& lhs, const VectorValues& rhs) {
|
||||
return lhs.equals(rhs);
|
||||
}
|
||||
|
||||
/// @{
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -510,17 +510,12 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
|
|||
GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S,
|
||||
size_t name2, gtsam::Matrix T,
|
||||
const gtsam::noiseModel::Diagonal* sigmas);
|
||||
GaussianConditional(const vector<std::pair<gtsam::Key, gtsam::Matrix>> terms,
|
||||
size_t nrFrontals, gtsam::Vector d,
|
||||
const gtsam::noiseModel::Diagonal* sigmas);
|
||||
|
||||
// Constructors with no noise model
|
||||
GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R);
|
||||
GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S);
|
||||
GaussianConditional(size_t key, gtsam::Vector d, gtsam::Matrix R, size_t name1, gtsam::Matrix S,
|
||||
size_t name2, gtsam::Matrix T);
|
||||
GaussianConditional(const gtsam::KeyVector& keys, size_t nrFrontals,
|
||||
const gtsam::VerticalBlockMatrix& augmentedMatrix);
|
||||
|
||||
// Named constructors
|
||||
static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,
|
||||
|
|
|
|||
|
|
@ -516,7 +516,6 @@ TEST(GaussianConditional, Print) {
|
|||
" d = [ 20 40 ]\n"
|
||||
" mean: 1 elements\n"
|
||||
" x0: 20 40\n"
|
||||
" logNormalizationConstant: -4.0351\n"
|
||||
"isotropic dim=2 sigma=3\n";
|
||||
EXPECT(assert_print_equal(expected, conditional, "GaussianConditional"));
|
||||
|
||||
|
|
@ -531,7 +530,6 @@ TEST(GaussianConditional, Print) {
|
|||
" S[x1] = [ -1 -2 ]\n"
|
||||
" [ -3 -4 ]\n"
|
||||
" d = [ 20 40 ]\n"
|
||||
" logNormalizationConstant: -4.0351\n"
|
||||
"isotropic dim=2 sigma=3\n";
|
||||
EXPECT(assert_print_equal(expected1, conditional1, "GaussianConditional"));
|
||||
|
||||
|
|
@ -547,7 +545,6 @@ TEST(GaussianConditional, Print) {
|
|||
" S[y1] = [ -5 -6 ]\n"
|
||||
" [ -7 -8 ]\n"
|
||||
" d = [ 20 40 ]\n"
|
||||
" logNormalizationConstant: -4.0351\n"
|
||||
"isotropic dim=2 sigma=3\n";
|
||||
EXPECT(assert_print_equal(expected2, conditional2, "GaussianConditional"));
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue