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

HybridGaussianFactor tests passing

release/4.3a0
Varun Agrawal 2024-09-13 17:37:29 -04:00
parent 4343b3aadc
commit 0bbf16bf4b
6 changed files with 71 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -220,22 +220,19 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
const DiscreteKeys discreteParentKeys = discreteKeys();
const KeyVector continuousParentKeys = continuousParents();
const HybridGaussianFactor::Factors likelihoods(
conditionals_, [&](const GaussianConditional::shared_ptr &conditional) {
const auto likelihood_m = conditional->likelihood(given);
conditionals_,
[&](const GaussianConditional::shared_ptr &conditional)
-> std::pair<GaussianFactor::shared_ptr, double> {
auto likelihood_m = conditional->likelihood(given);
const double Cgm_Kgcm =
logConstant_ - conditional->logNormalizationConstant();
if (Cgm_Kgcm == 0.0) {
return likelihood_m;
return {likelihood_m, 0.0};
} else {
// Add a constant factor to the likelihood in case the noise models
// are not all equal.
GaussianFactorGraph gfg;
gfg.push_back(likelihood_m);
Vector c(1);
c << std::sqrt(2.0 * Cgm_Kgcm);
auto constantFactor = std::make_shared<JacobianFactor>(c);
gfg.push_back(constantFactor);
return std::make_shared<JacobianFactor>(gfg);
double c = std::sqrt(2.0 * Cgm_Kgcm);
return {likelihood_m, c};
}
});
return std::make_shared<HybridGaussianFactor>(

28
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -46,8 +46,10 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
// Check the base and the factors:
return Base::equals(*e, tol) &&
factors_.equals(e->factors_,
[tol](const sharedFactor &f1, const sharedFactor &f2) {
return f1->equals(*f2, tol);
[tol](const std::pair<sharedFactor, double> &f1,
const std::pair<sharedFactor, double> &f2) {
return f1.first->equals(*f2.first, tol) &&
(f1.second == f2.second);
});
}
@ -63,11 +65,13 @@ void HybridGaussianFactor::print(const std::string &s,
} else {
factors_.print(
"", [&](Key k) { return formatter(k); },
[&](const sharedFactor &gf) -> std::string {
[&](const std::pair<sharedFactor, double> &gfv) -> std::string {
auto [gf, val] = gfv;
RedirectCout rd;
std::cout << ":\n";
if (gf) {
gf->print("", formatter);
std::cout << "value: " << val << std::endl;
return rd.str();
} else {
return "nullptr";
@ -78,8 +82,8 @@ void HybridGaussianFactor::print(const std::string &s,
}
/* *******************************************************************************/
HybridGaussianFactor::sharedFactor HybridGaussianFactor::operator()(
const DiscreteValues &assignment) const {
std::pair<HybridGaussianFactor::sharedFactor, double>
HybridGaussianFactor::operator()(const DiscreteValues &assignment) const {
return factors_(assignment);
}
@ -99,7 +103,9 @@ GaussianFactorGraphTree HybridGaussianFactor::add(
/* *******************************************************************************/
GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree()
const {
auto wrap = [](const sharedFactor &gf) { return GaussianFactorGraph{gf}; };
auto wrap = [](const std::pair<sharedFactor, double> &gfv) {
return GaussianFactorGraph{gfv.first};
};
return {factors_, wrap};
}
@ -107,8 +113,10 @@ GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree()
AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
const VectorValues &continuousValues) const {
// functor to convert from sharedFactor to double error value.
auto errorFunc = [&continuousValues](const sharedFactor &gf) {
return gf->error(continuousValues);
auto errorFunc =
[&continuousValues](const std::pair<sharedFactor, double> &gfv) {
auto [gf, val] = gfv;
return gf->error(continuousValues) + val;
};
DecisionTree<Key, double> error_tree(factors_, errorFunc);
return error_tree;
@ -116,8 +124,8 @@ AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
/* *******************************************************************************/
double HybridGaussianFactor::error(const HybridValues &values) const {
const sharedFactor gf = factors_(values.discrete());
return gf->error(values.continuous());
auto &&[gf, val] = factors_(values.discrete());
return gf->error(values.continuous()) + val;
}
} // namespace gtsam

19
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -53,7 +53,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
using sharedFactor = std::shared_ptr<GaussianFactor>;
/// typedef for Decision Tree of Gaussian factors and log-constant.
using Factors = DecisionTree<Key, sharedFactor>;
using Factors = DecisionTree<Key, std::pair<sharedFactor, double>>;
private:
/// Decision tree of Gaussian factors indexed by discrete keys.
@ -80,8 +80,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
* @param continuousKeys A vector of keys representing continuous variables.
* @param discreteKeys A vector of keys representing discrete variables and
* their cardinalities.
* @param factors The decision tree of Gaussian factors stored
* as the mixture density.
* @param factors The decision tree of Gaussian factors and arbitrary scalars.
*/
HybridGaussianFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
@ -93,11 +92,12 @@ class GTSAM_EXPORT HybridGaussianFactor : 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 factors Vector of gaussian factor shared pointers
* and arbitrary scalars.
*/
HybridGaussianFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
const std::vector<sharedFactor> &factors)
HybridGaussianFactor(
const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
const std::vector<std::pair<sharedFactor, double>> &factors)
: HybridGaussianFactor(continuousKeys, discreteKeys,
Factors(discreteKeys, factors)) {}
@ -114,8 +114,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
/// @name Standard API
/// @{
/// Get factor at a given discrete assignment.
sharedFactor operator()(const DiscreteValues &assignment) const;
/// Get the factor and scalar at a given discrete assignment.
std::pair<sharedFactor, double> operator()(
const DiscreteValues &assignment) const;
/**
* @brief Combine the Gaussian Factor Graphs in `sum` and `this` while

34
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -92,13 +92,15 @@ void HybridGaussianFactorGraph::printErrors(
// Clear the stringstream
ss.str(std::string());
if (auto gmf = std::dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
if (auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(factor)) {
if (factor == nullptr) {
std::cout << "nullptr"
<< "\n";
} else {
gmf->operator()(values.discrete())->print(ss.str(), keyFormatter);
std::cout << "error = " << gmf->error(values) << std::endl;
auto [factor, val] = hgf->operator()(values.discrete());
factor->print(ss.str(), keyFormatter);
std::cout << "value: " << val << std::endl;
std::cout << "error = " << factor->error(values) << std::endl;
}
} else if (auto hc = std::dynamic_pointer_cast<HybridConditional>(factor)) {
if (factor == nullptr) {
@ -262,9 +264,12 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
// Case where we have a HybridGaussianFactor with no continuous keys.
// In this case, compute discrete probabilities.
auto logProbability =
[&](const GaussianFactor::shared_ptr &factor) -> double {
if (!factor) return 0.0;
return -factor->error(VectorValues());
[&](const std::pair<GaussianFactor::shared_ptr, double> &fv)
-> double {
auto [factor, val] = fv;
double v = 0.5 * val * val;
if (!factor) return -v;
return -(factor->error(VectorValues()) + v);
};
AlgebraicDecisionTree<Key> logProbabilities =
DecisionTree<Key, double>(gmf->factors(), logProbability);
@ -348,7 +353,8 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
const KeyVector &continuousSeparator,
const DiscreteKeys &discreteSeparator) {
// Correct for the normalization constant used up by the conditional
auto correct = [&](const Result &pair) -> GaussianFactor::shared_ptr {
auto correct =
[&](const Result &pair) -> std::pair<GaussianFactor::shared_ptr, double> {
const auto &[conditional, factor] = pair;
if (factor) {
auto hf = std::dynamic_pointer_cast<HessianFactor>(factor);
@ -357,10 +363,10 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
// as per the Hessian definition
hf->constantTerm() += 2.0 * conditional->logNormalizationConstant();
}
return factor;
return {factor, 0.0};
};
DecisionTree<Key, GaussianFactor::shared_ptr> newFactors(eliminationResults,
correct);
DecisionTree<Key, std::pair<GaussianFactor::shared_ptr, double>> newFactors(
eliminationResults, correct);
return std::make_shared<HybridGaussianFactor>(continuousSeparator,
discreteSeparator, newFactors);
@ -597,10 +603,10 @@ GaussianFactorGraph HybridGaussianFactorGraph::operator()(
gfg.push_back(gf);
} else if (auto gc = std::dynamic_pointer_cast<GaussianConditional>(f)) {
gfg.push_back(gf);
} else if (auto gmf = std::dynamic_pointer_cast<HybridGaussianFactor>(f)) {
gfg.push_back((*gmf)(assignment));
} else if (auto gm = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
gfg.push_back((*gm)(assignment));
} else if (auto hgf = std::dynamic_pointer_cast<HybridGaussianFactor>(f)) {
gfg.push_back((*hgf)(assignment).first);
} else if (auto hgc = dynamic_pointer_cast<HybridGaussianConditional>(f)) {
gfg.push_back((*hgc)(assignment));
} else {
continue;
}

3
gtsam/hybrid/HybridNonlinearFactor.h
View File

@ -245,7 +245,8 @@ class HybridNonlinearFactor : public HybridFactor {
const Values& continuousValues) const {
// functional to linearize each factor in the decision tree
auto linearizeDT =
[continuousValues](const std::pair<sharedFactor, double>& f) {
[continuousValues](const std::pair<sharedFactor, double>& f)
-> std::pair<GaussianFactor::shared_ptr, double> {
auto [factor, val] = f;
return {factor->linearize(continuousValues), val};
};

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

@ -71,8 +71,10 @@ TEST(HybridGaussianFactor, Sum) {
auto f20 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
auto f21 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
auto f22 = std::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
std::vector<GaussianFactor::shared_ptr> factorsA{f10, f11};
std::vector<GaussianFactor::shared_ptr> factorsB{f20, f21, f22};
std::vector<std::pair<GaussianFactor::shared_ptr, double>> factorsA{
{f10, 0.0}, {f11, 0.0}};
std::vector<std::pair<GaussianFactor::shared_ptr, double>> factorsB{
{f20, 0.0}, {f21, 0.0}, {f22, 0.0}};
// TODO(Frank): why specify keys at all? And: keys in factor should be *all*
// keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey?
@ -109,7 +111,8 @@ TEST(HybridGaussianFactor, Printing) {
auto b = Matrix::Zero(2, 1);
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);
std::vector<GaussianFactor::shared_ptr> factors{f10, f11};
std::vector<std::pair<GaussianFactor::shared_ptr, double>> factors{
{f10, 0.0}, {f11, 0.0}};
HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
@ -128,6 +131,7 @@ Hybrid [x1 x2; 1]{
]
b = [ 0 0 ]
No noise model
value: 0
1 Leaf :
A[x1] = [
@ -140,6 +144,7 @@ Hybrid [x1 x2; 1]{
]
b = [ 0 0 ]
No noise model
value: 0
}
)";
@ -178,7 +183,8 @@ TEST(HybridGaussianFactor, Error) {
auto f0 = std::make_shared<JacobianFactor>(X(1), A01, X(2), A02, b);
auto f1 = std::make_shared<JacobianFactor>(X(1), A11, X(2), A12, b);
std::vector<GaussianFactor::shared_ptr> factors{f0, f1};
std::vector<std::pair<GaussianFactor::shared_ptr, double>> factors{{f0, 0.0},
{f1, 0.0}};
HybridGaussianFactor mixtureFactor({X(1), X(2)}, {m1}, factors);