Skip to content

Commit

Permalink
Minimize formatting changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@dellaert
dellaert committed Oct 8, 2024
1 parent 88b8dc9 commit 9f7ccbb
Show file tree
Hide file tree
Showing 17 changed files with 509 additions and 385 deletions.
154 changes: 81 additions & 73 deletions gtsam/hybrid/HybridGaussianConditional.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,8 @@ namespace gtsam {
* - nrFrontals: Optional size_t for number of frontal variables
* - pairs: FactorValuePairs for storing conditionals with their negLogConstant
* - conditionals: Conditionals for storing conditionals. TODO(frank): kill!
* - minNegLogConstant: minimum negLogConstant, computed here, subtracted in constructor
* - minNegLogConstant: minimum negLogConstant, computed here, subtracted in
* constructor
*/
struct HybridGaussianConditional::Helper {
std::optional<size_t> nrFrontals;
Expand All @@ -53,16 +54,17 @@ struct HybridGaussianConditional::Helper {

/// Construct from a vector of mean and sigma pairs, plus extra args.
template <typename... Args>
explicit Helper(const DiscreteKey& mode, const P& p, Args&&... args) {
explicit Helper(const DiscreteKey &mode, const P &p, Args &&...args) {
nrFrontals = 1;
minNegLogConstant = std::numeric_limits<double>::infinity();

std::vector<GaussianFactorValuePair> fvs;
std::vector<GC::shared_ptr> gcs;
fvs.reserve(p.size());
gcs.reserve(p.size());
for (auto&& [mean, sigma] : p) {
auto gaussianConditional = GC::sharedMeanAndStddev(std::forward<Args>(args)..., mean, sigma);
for (auto &&[mean, sigma] : p) {
auto gaussianConditional =
GC::sharedMeanAndStddev(std::forward<Args>(args)..., mean, sigma);
double value = gaussianConditional->negLogConstant();
minNegLogConstant = std::min(minNegLogConstant, value);
fvs.emplace_back(gaussianConditional, value);
Expand All @@ -74,8 +76,9 @@ struct HybridGaussianConditional::Helper {
}

/// Construct from tree of GaussianConditionals.
explicit Helper(const Conditionals& conditionals)
: conditionals(conditionals), minNegLogConstant(std::numeric_limits<double>::infinity()) {
explicit Helper(const Conditionals &conditionals)
: conditionals(conditionals),
minNegLogConstant(std::numeric_limits<double>::infinity()) {
auto func = [this](const GC::shared_ptr& gc) -> GaussianFactorValuePair {
if (!gc) return {nullptr, std::numeric_limits<double>::infinity()};
if (!nrFrontals) nrFrontals = gc->nrFrontals();
Expand All @@ -93,87 +96,85 @@ struct HybridGaussianConditional::Helper {
};

/* *******************************************************************************/
HybridGaussianConditional::HybridGaussianConditional(const DiscreteKeys& discreteParents,
const Helper& helper)
: BaseFactor(
discreteParents,
FactorValuePairs(
helper.pairs,
[&](const GaussianFactorValuePair& pair) { // subtract minNegLogConstant
return GaussianFactorValuePair{pair.first, pair.second - helper.minNegLogConstant};
})),
HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKeys& discreteParents, const Helper& helper)
: BaseFactor(discreteParents,
FactorValuePairs(helper.pairs,
[&](const GaussianFactorValuePair&
pair) { // subtract minNegLogConstant
return GaussianFactorValuePair{
pair.first,
pair.second - helper.minNegLogConstant};
})),
BaseConditional(*helper.nrFrontals),
conditionals_(helper.conditionals),
negLogConstant_(helper.minNegLogConstant) {}

HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey& discreteParent,
const std::vector<GaussianConditional::shared_ptr>& conditionals)
const DiscreteKey &discreteParent,
const std::vector<GaussianConditional::shared_ptr> &conditionals)
: HybridGaussianConditional(DiscreteKeys{discreteParent},
Conditionals({discreteParent}, conditionals)) {}

HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey& discreteParent,
Key key, //
const std::vector<std::pair<Vector, double>>& parameters)
const DiscreteKey &discreteParent, Key key, //
const std::vector<std::pair<Vector, double>> &parameters)
: HybridGaussianConditional(DiscreteKeys{discreteParent},
Helper(discreteParent, parameters, key)) {}

HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey& discreteParent,
Key key, //
const Matrix& A,
Key parent,
const std::vector<std::pair<Vector, double>>& parameters)
: HybridGaussianConditional(DiscreteKeys{discreteParent},
Helper(discreteParent, parameters, key, A, parent)) {}
const DiscreteKey &discreteParent, Key key, //
const Matrix &A, Key parent,
const std::vector<std::pair<Vector, double>> &parameters)
: HybridGaussianConditional(
DiscreteKeys{discreteParent},
Helper(discreteParent, parameters, key, A, parent)) {}

HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKey& discreteParent,
Key key, //
const Matrix& A1,
Key parent1,
const Matrix& A2,
Key parent2,
const std::vector<std::pair<Vector, double>>& parameters)
: HybridGaussianConditional(DiscreteKeys{discreteParent},
Helper(discreteParent, parameters, key, A1, parent1, A2, parent2)) {
}
const DiscreteKey &discreteParent, Key key, //
const Matrix &A1, Key parent1, const Matrix &A2, Key parent2,
const std::vector<std::pair<Vector, double>> &parameters)
: HybridGaussianConditional(
DiscreteKeys{discreteParent},
Helper(discreteParent, parameters, key, A1, parent1, A2, parent2)) {}

HybridGaussianConditional::HybridGaussianConditional(
const DiscreteKeys& discreteParents,
const HybridGaussianConditional::Conditionals& conditionals)
const DiscreteKeys &discreteParents,
const HybridGaussianConditional::Conditionals &conditionals)
: HybridGaussianConditional(discreteParents, Helper(conditionals)) {}

/* *******************************************************************************/
const HybridGaussianConditional::Conditionals& HybridGaussianConditional::conditionals() const {
const HybridGaussianConditional::Conditionals &
HybridGaussianConditional::conditionals() const {
return conditionals_;
}

/* *******************************************************************************/
size_t HybridGaussianConditional::nrComponents() const {
size_t total = 0;
conditionals_.visit([&total](const GaussianFactor::shared_ptr& node) {
conditionals_.visit([&total](const GaussianFactor::shared_ptr &node) {
if (node) total += 1;
});
return total;
}

/* *******************************************************************************/
GaussianConditional::shared_ptr HybridGaussianConditional::choose(
const DiscreteValues& discreteValues) const {
auto& ptr = conditionals_(discreteValues);
const DiscreteValues &discreteValues) const {
auto &ptr = conditionals_(discreteValues);
if (!ptr) return nullptr;
auto conditional = std::dynamic_pointer_cast<GaussianConditional>(ptr);
if (conditional)
return conditional;
else
throw std::logic_error("A HybridGaussianConditional unexpectedly contained a non-conditional");
throw std::logic_error(
"A HybridGaussianConditional unexpectedly contained a non-conditional");
}

/* *******************************************************************************/
bool HybridGaussianConditional::equals(const HybridFactor& lf, double tol) const {
const This* e = dynamic_cast<const This*>(&lf);
bool HybridGaussianConditional::equals(const HybridFactor &lf,
double tol) const {
const This *e = dynamic_cast<const This *>(&lf);
if (e == nullptr) return false;

// This will return false if either conditionals_ is empty or e->conditionals_
Expand All @@ -182,26 +183,27 @@ bool HybridGaussianConditional::equals(const HybridFactor& lf, double tol) const

// Check the base and the factors:
return BaseFactor::equals(*e, tol) &&
conditionals_.equals(e->conditionals_, [tol](const auto& f1, const auto& f2) {
return (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol));
});
conditionals_.equals(
e->conditionals_, [tol](const auto &f1, const auto &f2) {
return (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol));
});
}

/* *******************************************************************************/
void HybridGaussianConditional::print(const std::string& s, const KeyFormatter& formatter) const {
void HybridGaussianConditional::print(const std::string &s,
const KeyFormatter &formatter) const {
std::cout << (s.empty() ? "" : s + "\n");
BaseConditional::print("", formatter);
std::cout << " Discrete Keys = ";
for (auto& dk : discreteKeys()) {
for (auto &dk : discreteKeys()) {
std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), ";
}
std::cout << std::endl
<< " logNormalizationConstant: " << -negLogConstant() << std::endl
<< std::endl;
conditionals_.print(
"",
[&](Key k) { return formatter(k); },
[&](const GaussianConditional::shared_ptr& gf) -> std::string {
"", [&](Key k) { return formatter(k); },
[&](const GaussianConditional::shared_ptr &gf) -> std::string {
RedirectCout rd;
if (gf && !gf->empty()) {
gf->print("", formatter);
Expand All @@ -218,19 +220,20 @@ KeyVector HybridGaussianConditional::continuousParents() const {
const auto range = parents();
KeyVector continuousParentKeys(range.begin(), range.end());
// Loop over all discrete keys:
for (const auto& discreteKey : discreteKeys()) {
for (const auto &discreteKey : discreteKeys()) {
const Key key = discreteKey.first;
// remove that key from continuousParentKeys:
continuousParentKeys.erase(
std::remove(continuousParentKeys.begin(), continuousParentKeys.end(), key),
continuousParentKeys.erase(std::remove(continuousParentKeys.begin(),
continuousParentKeys.end(), key),
continuousParentKeys.end());
}
return continuousParentKeys;
}

/* ************************************************************************* */
bool HybridGaussianConditional::allFrontalsGiven(const VectorValues& given) const {
for (auto&& kv : given) {
bool HybridGaussianConditional::allFrontalsGiven(
const VectorValues &given) const {
for (auto &&kv : given) {
if (given.find(kv.first) == given.end()) {
return false;
}
Expand All @@ -240,7 +243,7 @@ bool HybridGaussianConditional::allFrontalsGiven(const VectorValues& given) cons

/* ************************************************************************* */
std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
const VectorValues& given) const {
const VectorValues &given) const {
if (!allFrontalsGiven(given)) {
throw std::runtime_error(
"HybridGaussianConditional::likelihood: given values are missing some "
Expand All @@ -251,54 +254,59 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
const KeyVector continuousParentKeys = continuousParents();
const HybridGaussianFactor::FactorValuePairs likelihoods(
conditionals_,
[&](const GaussianConditional::shared_ptr& conditional) -> GaussianFactorValuePair {
[&](const GaussianConditional::shared_ptr &conditional)
-> GaussianFactorValuePair {
const auto likelihood_m = conditional->likelihood(given);
const double Cgm_Kgcm = conditional->negLogConstant() - negLogConstant_;
return {likelihood_m, Cgm_Kgcm};
});
return std::make_shared<HybridGaussianFactor>(discreteParentKeys, likelihoods);
return std::make_shared<HybridGaussianFactor>(discreteParentKeys,
likelihoods);
}

/* ************************************************************************* */
std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys& discreteKeys) {
std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) {
std::set<DiscreteKey> s(discreteKeys.begin(), discreteKeys.end());
return s;
}

/* *******************************************************************************/
HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
const DecisionTreeFactor& discreteProbs) const {
const DecisionTreeFactor &discreteProbs) const {
// Find keys in discreteProbs.keys() but not in this->keys():
std::set<Key> mine(this->keys().begin(), this->keys().end());
std::set<Key> theirs(discreteProbs.keys().begin(), discreteProbs.keys().end());
std::set<Key> theirs(discreteProbs.keys().begin(),
discreteProbs.keys().end());
std::vector<Key> diff;
std::set_difference(
theirs.begin(), theirs.end(), mine.begin(), mine.end(), std::back_inserter(diff));
std::set_difference(theirs.begin(), theirs.end(), mine.begin(), mine.end(),
std::back_inserter(diff));

// Find maximum probability value for every combination of our keys.
Ordering keys(diff);
auto max = discreteProbs.max(keys);

// Check the max value for every combination of our keys.
// If the max value is 0.0, we can prune the corresponding conditional.
auto pruner =
[&](const Assignment<Key>& choices,
const GaussianConditional::shared_ptr& conditional) -> GaussianConditional::shared_ptr {
auto pruner = [&](const Assignment<Key> &choices,
const GaussianConditional::shared_ptr &conditional)
-> GaussianConditional::shared_ptr {
return (max->evaluate(choices) == 0.0) ? nullptr : conditional;
};

auto pruned_conditionals = conditionals_.apply(pruner);
return std::make_shared<HybridGaussianConditional>(discreteKeys(), pruned_conditionals);
return std::make_shared<HybridGaussianConditional>(discreteKeys(),
pruned_conditionals);
}

/* *******************************************************************************/
double HybridGaussianConditional::logProbability(const HybridValues& values) const {
double HybridGaussianConditional::logProbability(
const HybridValues &values) const {
auto conditional = conditionals_(values.discrete());
return conditional->logProbability(values.continuous());
}

/* *******************************************************************************/
double HybridGaussianConditional::evaluate(const HybridValues& values) const {
double HybridGaussianConditional::evaluate(const HybridValues &values) const {
auto conditional = conditionals_(values.discrete());
return conditional->evaluate(values.continuous());
}
Expand Down
Loading

0 comments on commit 9f7ccbb

Please sign in to comment.