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

Merge pull request #1865 from borglab/feature/no_hiding-2 

Updates to `No Hiding` PR
release/4.3a0
Frank Dellaert 2024-10-09 13:37:30 +09:00 committed by GitHub
parent 9f7ccbb33c 436524a4df
commit 59f97d64eb
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
14 changed files with 141 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/hybrid/HybridBayesTree.cpp
View File

@ -22,14 +22,13 @@
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridBayesTree.h>
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/inference/BayesTree-inst.h>
#include <gtsam/inference/BayesTreeCliqueBase-inst.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <memory>
#include "gtsam/hybrid/HybridConditional.h"
namespace gtsam {
// Instantiate base class

1
gtsam/hybrid/HybridConditional.cpp
View File

@ -13,6 +13,7 @@
* @file HybridConditional.cpp
* @date Mar 11, 2022
* @author Fan Jiang
* @author Varun Agrawal
*/
#include <gtsam/hybrid/HybridConditional.h>

1
gtsam/hybrid/HybridConditional.h
View File

@ -13,6 +13,7 @@
* @file HybridConditional.h
* @date Mar 11, 2022
* @author Fan Jiang
* @author Varun Agrawal
*/
#pragma once

8
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -183,9 +183,11 @@ bool HybridGaussianConditional::equals(const HybridFactor &lf,
// 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 GaussianConditional::shared_ptr &f1,
const GaussianConditional::shared_ptr &f2) {
return (!f1 && !f2) ||
(f1 && f2 && f1->equals(*f2, tol));
});
}

2
gtsam/hybrid/HybridGaussianConditional.h
View File

@ -24,9 +24,9 @@
#include <gtsam/discrete/DecisionTree.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/hybrid/HybridGaussianProductFactor.h>
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianProductFactor.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/linear/GaussianConditional.h>

32
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -51,7 +51,7 @@ struct HybridGaussianFactor::ConstructorHelper {
// Build the FactorValuePairs DecisionTree
pairs = FactorValuePairs(
DecisionTree<Key, GaussianFactor::shared_ptr>(discreteKeys, factors),
[](const auto& f) {
[](const sharedFactor& f) {
return std::pair{f,
f ? 0.0 : std::numeric_limits<double>::infinity()};
});
@ -63,7 +63,7 @@ struct HybridGaussianFactor::ConstructorHelper {
const std::vector<GaussianFactorValuePair>& factorPairs)
: discreteKeys({discreteKey}) {
// Extract continuous keys from the first non-null factor
for (const auto& pair : factorPairs) {
for (const GaussianFactorValuePair& pair : factorPairs) {
if (pair.first && continuousKeys.empty()) {
continuousKeys = pair.first->keys();
break;
@ -121,7 +121,8 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
if (factors_.empty() ^ e->factors_.empty()) return false;
// Check the base and the factors:
auto compareFunc = [tol](const auto& pair1, const auto& pair2) {
auto compareFunc = [tol](const GaussianFactorValuePair& pair1,
const GaussianFactorValuePair& pair2) {
auto f1 = pair1.first, f2 = pair2.first;
bool match = (!f1 && !f2) || (f1 && f2 && f1->equals(*f2, tol));
return match && gtsam::equal(pair1.second, pair2.second, tol);
@ -139,9 +140,8 @@ void HybridGaussianFactor::print(const std::string &s,
std::cout << " empty" << std::endl;
} else {
factors_.print(
"",
[&](Key k) { return formatter(k); },
[&](const auto& pair) -> std::string {
"", [&](Key k) { return formatter(k); },
[&](const GaussianFactorValuePair& pair) -> std::string {
RedirectCout rd;
std::cout << ":\n";
if (pair.first) {
@ -169,18 +169,25 @@ HybridGaussianProductFactor HybridGaussianFactor::asProductFactor() const {
// - Each leaf converted to a GaussianFactorGraph with just the factor and its
// scalar.
return {{factors_,
[](const auto& pair) -> std::pair<GaussianFactorGraph, double> {
[](const GaussianFactorValuePair& pair)
-> std::pair<GaussianFactorGraph, double> {
return {GaussianFactorGraph{pair.first}, pair.second};
}}};
}
/* *******************************************************************************/
inline static double PotentiallyPrunedComponentError(
const GaussianFactorValuePair& pair, const VectorValues& continuousValues) {
return pair.first ? pair.first->error(continuousValues) + pair.second
: std::numeric_limits<double>::infinity();
}
/* *******************************************************************************/
AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
const VectorValues& continuousValues) const {
// functor to convert from sharedFactor to double error value.
auto errorFunc = [&continuousValues](const auto& pair) {
return pair.first ? pair.first->error(continuousValues) + pair.second
: std::numeric_limits<double>::infinity();
auto errorFunc = [&continuousValues](const GaussianFactorValuePair& pair) {
return PotentiallyPrunedComponentError(pair, continuousValues);
};
DecisionTree<Key, double> error_tree(factors_, errorFunc);
return error_tree;
@ -189,9 +196,8 @@ AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
/* *******************************************************************************/
double HybridGaussianFactor::error(const HybridValues& values) const {
// Directly index to get the component, no need to build the whole tree.
const auto pair = factors_(values.discrete());
return pair.first ? pair.first->error(values.continuous()) + pair.second
: std::numeric_limits<double>::infinity();
const GaussianFactorValuePair pair = factors_(values.discrete());
return PotentiallyPrunedComponentError(pair, values.continuous());
}
} // namespace gtsam

9
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -120,9 +120,8 @@ public:
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 = "", const KeyFormatter &formatter =
DefaultKeyFormatter) const override;
/// @}
/// @name Standard API
@ -138,8 +137,8 @@ public:
* @return AlgebraicDecisionTree<Key> A decision tree with the same keys
* as the factors involved, and leaf values as the error.
*/
AlgebraicDecisionTree<Key>
errorTree(const VectorValues &continuousValues) const override;
AlgebraicDecisionTree<Key> errorTree(
const VectorValues &continuousValues) const override;
/**
* @brief Compute the log-likelihood, including the log-normalizing constant.

20
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -24,6 +24,7 @@
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteJunctionTree.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridConditional.h>
#include <gtsam/hybrid/HybridEliminationTree.h>
#include <gtsam/hybrid/HybridFactor.h>
@ -39,7 +40,6 @@
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include "gtsam/discrete/DiscreteValues.h"
#include <cstddef>
#include <iostream>
@ -57,7 +57,8 @@ using std::dynamic_pointer_cast;
using OrphanWrapper = BayesTreeOrphanWrapper<HybridBayesTree::Clique>;
using Result =
std::pair<std::shared_ptr<GaussianConditional>, GaussianFactor::shared_ptr>;
using ResultTree = DecisionTree<Key, std::pair<Result, double>>;
using ResultValuePair = std::pair<Result, double>;
using ResultTree = DecisionTree<Key, ResultValuePair>;
static const VectorValues kEmpty;
@ -83,11 +84,12 @@ static void printFactor(const std::shared_ptr<Factor> &factor,
const DiscreteValues &assignment,
const KeyFormatter &keyFormatter) {
if (auto hgf = dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
if (assignment.empty())
if (assignment.empty()) {
hgf->print("HybridGaussianFactor:", keyFormatter);
else
} else {
hgf->operator()(assignment)
.first->print("HybridGaussianFactor, component:", keyFormatter);
}
} else if (auto gf = dynamic_pointer_cast<GaussianFactor>(factor)) {
factor->print("GaussianFactor:\n", keyFormatter);
@ -99,13 +101,14 @@ static void printFactor(const std::shared_ptr<Factor> &factor,
} else if (hc->isDiscrete()) {
factor->print("DiscreteConditional:\n", keyFormatter);
} else {
if (assignment.empty())
if (assignment.empty()) {
hc->print("HybridConditional:", keyFormatter);
else
} else {
hc->asHybrid()
->choose(assignment)
->print("HybridConditional, component:\n", keyFormatter);
}
}
} else {
factor->print("Unknown factor type\n", keyFormatter);
}
@ -303,7 +306,7 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
const ResultTree &eliminationResults,
const DiscreteKeys &discreteSeparator) {
// Correct for the normalization constant used up by the conditional
auto correct = [&](const auto &pair) -> GaussianFactorValuePair {
auto correct = [&](const ResultValuePair &pair) -> GaussianFactorValuePair {
const auto &[conditional, factor] = pair.first;
const double scalar = pair.second;
if (conditional && factor) {
@ -382,7 +385,8 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
// Create the HybridGaussianConditional from the conditionals
HybridGaussianConditional::Conditionals conditionals(
eliminationResults, [](const auto& pair) { return pair.first.first; });
eliminationResults,
[](const ResultValuePair &pair) { return pair.first.first; });
auto hybridGaussian = std::make_shared<HybridGaussianConditional>(
discreteSeparator, conditionals);

4
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -145,8 +145,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
/// @name Testable
/// @{
void
print(const std::string &s = "HybridGaussianFactorGraph",
void print(
const std::string& s = "HybridGaussianFactorGraph",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/**

30
gtsam/hybrid/HybridGaussianProductFactor.cpp
View File

@ -22,43 +22,52 @@
#include <gtsam/hybrid/HybridGaussianProductFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <string>
namespace gtsam {
using Y = HybridGaussianProductFactor::Y;
using Y = GaussianFactorGraphValuePair;
/* *******************************************************************************/
static Y add(const Y& y1, const Y& y2) {
GaussianFactorGraph result = y1.first;
result.push_back(y2.first);
return {result, y1.second + y2.second};
};
/* *******************************************************************************/
HybridGaussianProductFactor operator+(const HybridGaussianProductFactor& a,
const HybridGaussianProductFactor& b) {
return a.empty() ? b : HybridGaussianProductFactor(a.apply(b, add));
}
/* *******************************************************************************/
HybridGaussianProductFactor HybridGaussianProductFactor::operator+(
const HybridGaussianFactor& factor) const {
return *this + factor.asProductFactor();
}
/* *******************************************************************************/
HybridGaussianProductFactor HybridGaussianProductFactor::operator+(
const GaussianFactor::shared_ptr& factor) const {
return *this + HybridGaussianProductFactor(factor);
}
/* *******************************************************************************/
HybridGaussianProductFactor& HybridGaussianProductFactor::operator+=(
const GaussianFactor::shared_ptr& factor) {
*this = *this + factor;
return *this;
}
/* *******************************************************************************/
HybridGaussianProductFactor& HybridGaussianProductFactor::operator+=(
const HybridGaussianFactor& factor) {
*this = *this + factor;
return *this;
}
/* *******************************************************************************/
void HybridGaussianProductFactor::print(const std::string& s,
const KeyFormatter& formatter) const {
KeySet keys;
@ -69,18 +78,25 @@ void HybridGaussianProductFactor::print(const std::string& s,
};
Base::print(s, formatter, printer);
if (!keys.empty()) {
std::stringstream ss;
ss << s << " Keys:";
for (auto&& key : keys) ss << " " << formatter(key);
std::cout << ss.str() << "." << std::endl;
std::cout << s << " Keys:";
for (auto&& key : keys) std::cout << " " << formatter(key);
std::cout << "." << std::endl;
}
}
/* *******************************************************************************/
bool HybridGaussianProductFactor::equals(
const HybridGaussianProductFactor& other, double tol) const {
return Base::equals(other, [tol](const Y& a, const Y& b) {
return a.first.equals(b.first, tol) && std::abs(a.second - b.second) < tol;
});
}
/* *******************************************************************************/
HybridGaussianProductFactor HybridGaussianProductFactor::removeEmpty() const {
auto emptyGaussian = [](const Y& y) {
bool hasNull =
std::any_of(y.first.begin(),
y.first.end(),
std::any_of(y.first.begin(), y.first.end(),
[](const GaussianFactor::shared_ptr& ptr) { return !ptr; });
return hasNull ? Y{GaussianFactorGraph(), 0.0} : y;
};

18
gtsam/hybrid/HybridGaussianProductFactor.h
View File

@ -26,12 +26,13 @@ namespace gtsam {
class HybridGaussianFactor;
using GaussianFactorGraphValuePair = std::pair<GaussianFactorGraph, double>;
/// Alias for DecisionTree of GaussianFactorGraphs and their scalar sums
class HybridGaussianProductFactor
: public DecisionTree<Key, std::pair<GaussianFactorGraph, double>> {
class GTSAM_EXPORT HybridGaussianProductFactor
: public DecisionTree<Key, GaussianFactorGraphValuePair> {
public:
using Y = std::pair<GaussianFactorGraph, double>;
using Base = DecisionTree<Key, Y>;
using Base = DecisionTree<Key, GaussianFactorGraphValuePair>;
/// @name Constructors
/// @{
@ -46,7 +47,7 @@ class HybridGaussianProductFactor
*/
template <class FACTOR>
HybridGaussianProductFactor(const std::shared_ptr<FACTOR>& factor)
: Base(Y{GaussianFactorGraph{factor}, 0.0}) {}
: Base(GaussianFactorGraphValuePair{GaussianFactorGraph{factor}, 0.0}) {}
/**
* @brief Construct from DecisionTree
@ -94,12 +95,7 @@ class HybridGaussianProductFactor
* @return true if equal, false otherwise
*/
bool equals(const HybridGaussianProductFactor& other,
double tol = 1e-9) const {
return Base::equals(other, [tol](const Y& a, const Y& b) {
return a.first.equals(b.first, tol) &&
std::abs(a.second - b.second) < tol;
});
}
double tol = 1e-9) const;
/// @}

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

@ -199,7 +199,7 @@ TEST(HybridBayesNet, Tiny) {
factors_x0.push_back(fg.at(1));
auto productFactor = factors_x0.collectProductFactor();
// Check that scalars are 0 and 1.79
// Check that scalars are 0 and 1.79 (regression)
EXPECT_DOUBLES_EQUAL(0.0, productFactor({{M(0), 0}}).second, 1e-9);
EXPECT_DOUBLES_EQUAL(1.791759, productFactor({{M(0), 1}}).second, 1e-5);

29
gtsam/hybrid/tests/testHybridBayesTree.cpp
View File

@ -22,6 +22,8 @@
#include <gtsam/hybrid/HybridGaussianISAM.h>
#include <gtsam/inference/DotWriter.h>
#include <numeric>
#include "Switching.h"
// Include for test suite
@ -62,7 +64,8 @@ std::vector<GaussianFactor::shared_ptr> components(Key key) {
} // namespace two
/* ************************************************************************* */
TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
TEST(HybridGaussianFactorGraph,
HybridGaussianFactorGraphEliminateFullMultifrontalSimple) {
HybridGaussianFactorGraph hfg;
hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
@ -179,10 +182,8 @@ TEST(HybridGaussianFactorGraph, Switching) {
std::vector<int> naturalX(N);
std::iota(naturalX.begin(), naturalX.end(), 1);
std::vector<Key> ordX;
std::transform(
naturalX.begin(), naturalX.end(), std::back_inserter(ordX), [](int x) {
return X(x);
});
std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
[](int x) { return X(x); });
auto [ndX, lvls] = makeBinaryOrdering(ordX);
std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
@ -195,10 +196,8 @@ TEST(HybridGaussianFactorGraph, Switching) {
std::vector<int> naturalC(N - 1);
std::iota(naturalC.begin(), naturalC.end(), 1);
std::vector<Key> ordC;
std::transform(
naturalC.begin(), naturalC.end(), std::back_inserter(ordC), [](int x) {
return M(x);
});
std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
[](int x) { return M(x); });
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
const auto [ndC, lvls] = makeBinaryOrdering(ordC);
@ -237,10 +236,8 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
std::vector<int> naturalX(N);
std::iota(naturalX.begin(), naturalX.end(), 1);
std::vector<Key> ordX;
std::transform(
naturalX.begin(), naturalX.end(), std::back_inserter(ordX), [](int x) {
return X(x);
});
std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
[](int x) { return X(x); });
auto [ndX, lvls] = makeBinaryOrdering(ordX);
std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
@ -253,10 +250,8 @@ TEST(HybridGaussianFactorGraph, SwitchingISAM) {
std::vector<int> naturalC(N - 1);
std::iota(naturalC.begin(), naturalC.end(), 1);
std::vector<Key> ordC;
std::transform(
naturalC.begin(), naturalC.end(), std::back_inserter(ordC), [](int x) {
return M(x);
});
std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
[](int x) { return M(x); });
// std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
const auto [ndC, lvls] = makeBinaryOrdering(ordC);

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

@ -17,6 +17,8 @@
* @author Frank Dellaert
*/
#include <CppUnitLite/Test.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/Vector.h>
@ -37,9 +39,6 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/JacobianFactor.h>
#include <CppUnitLite/Test.h>
#include <CppUnitLite/TestHarness.h>
#include <cstddef>
#include <memory>
#include <vector>
@ -73,8 +72,8 @@ TEST(HybridGaussianFactorGraph, Creation) {
HybridGaussianConditional gm(
m0,
{std::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1), I_3x3),
std::make_shared<GaussianConditional>(
X(0), Vector3::Ones(), I_3x3, X(1), I_3x3)});
std::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3, X(1),
I_3x3)});
hfg.add(gm);
EXPECT_LONGS_EQUAL(2, hfg.size());
@ -118,8 +117,7 @@ TEST(HybridGaussianFactorGraph, hybridEliminationOneFactor) {
auto factor = std::dynamic_pointer_cast<DecisionTreeFactor>(result.second);
CHECK(factor);
// regression test
EXPECT(
assert_equal(DecisionTreeFactor{m1, "15.74961 15.74961"}, *factor, 1e-5));
EXPECT(assert_equal(DecisionTreeFactor{m1, "15.74961 15.74961"}, *factor, 1e-5));
}
/* ************************************************************************* */
@ -215,8 +213,8 @@ TEST(HybridBayesNet, Switching) {
// Check errorTree
AlgebraicDecisionTree<Key> actualErrors = graph.errorTree(continuousValues);
// Create expected error tree
const AlgebraicDecisionTree<Key> expectedErrors(
M(0), expectedError0, expectedError1);
const AlgebraicDecisionTree<Key> expectedErrors(M(0), expectedError0,
expectedError1);
// Check that the actual error tree matches the expected one
EXPECT(assert_equal(expectedErrors, actualErrors, 1e-5));
@ -232,8 +230,8 @@ TEST(HybridBayesNet, Switching) {
const AlgebraicDecisionTree<Key> graphPosterior =
graph.discretePosterior(continuousValues);
const double sum = probPrime0 + probPrime1;
const AlgebraicDecisionTree<Key> expectedPosterior(
M(0), probPrime0 / sum, probPrime1 / sum);
const AlgebraicDecisionTree<Key> expectedPosterior(M(0), probPrime0 / sum,
probPrime1 / sum);
EXPECT(assert_equal(expectedPosterior, graphPosterior, 1e-5));
// Make the clique of factors connected to x0:
@ -275,11 +273,9 @@ TEST(HybridBayesNet, Switching) {
// Check that the scalars incorporate the negative log constant of the
// conditional
EXPECT_DOUBLES_EQUAL(scalar0 - (*p_x0_given_x1_m)(modeZero)->negLogConstant(),
(*phi_x1_m)(modeZero).second,
1e-9);
(*phi_x1_m)(modeZero).second, 1e-9);
EXPECT_DOUBLES_EQUAL(scalar1 - (*p_x0_given_x1_m)(modeOne)->negLogConstant(),
(*phi_x1_m)(modeOne).second,
1e-9);
(*phi_x1_m)(modeOne).second, 1e-9);
// Check that the conditional and remaining factor are consistent for both
// modes