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

update HybridGaussianFactor to allow for tree of pairs

release/4.3a0
Varun Agrawal 2024-09-14 13:53:31 -04:00
parent f3b920257d
commit 5ceda1e157
5 changed files with 34 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -222,8 +222,8 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
const HybridGaussianFactor::Factors likelihoods(
conditionals_,
[&](const GaussianConditional::shared_ptr &conditional)
-> std::pair<GaussianFactor::shared_ptr, double> {
auto likelihood_m = conditional->likelihood(given);
-> GaussianFactorValuePair {
const auto likelihood_m = conditional->likelihood(given);
const double Cgm_Kgcm =
logConstant_ - conditional->logNormalizationConstant();
if (Cgm_Kgcm == 0.0) {
@ -231,8 +231,13 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
} else {
// Add a constant factor to the likelihood in case the noise models
// are not all equal.
double c = std::sqrt(2.0 * Cgm_Kgcm);
return {likelihood_m, c};
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), 0.0};
}
});
return std::make_shared<HybridGaussianFactor>(

27
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -45,12 +45,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 std::pair<sharedFactor, double> &f1,
const std::pair<sharedFactor, double> &f2) {
return f1.first->equals(*f2.first, tol) &&
(f1.second == f2.second);
});
factors_.equals(e->factors_, [tol](const GaussianFactorValuePair &f1,
const GaussianFactorValuePair &f2) {
return f1.first->equals(*f2.first, tol) && (f1.second == f2.second);
});
}
/* *******************************************************************************/
@ -65,7 +63,7 @@ void HybridGaussianFactor::print(const std::string &s,
} else {
factors_.print(
"", [&](Key k) { return formatter(k); },
[&](const std::pair<sharedFactor, double> &gfv) -> std::string {
[&](const GaussianFactorValuePair &gfv) -> std::string {
auto [gf, val] = gfv;
RedirectCout rd;
std::cout << ":\n";
@ -82,8 +80,8 @@ void HybridGaussianFactor::print(const std::string &s,
}
/* *******************************************************************************/
std::pair<HybridGaussianFactor::sharedFactor, double>
HybridGaussianFactor::operator()(const DiscreteValues &assignment) const {
GaussianFactorValuePair HybridGaussianFactor::operator()(
const DiscreteValues &assignment) const {
return factors_(assignment);
}
@ -103,7 +101,7 @@ GaussianFactorGraphTree HybridGaussianFactor::add(
/* *******************************************************************************/
GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree()
const {
auto wrap = [](const std::pair<sharedFactor, double> &gfv) {
auto wrap = [](const GaussianFactorValuePair &gfv) {
return GaussianFactorGraph{gfv.first};
};
return {factors_, wrap};
@ -113,11 +111,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 std::pair<sharedFactor, double> &gfv) {
auto [gf, val] = gfv;
return gf->error(continuousValues) + val;
};
auto errorFunc = [&continuousValues](const GaussianFactorValuePair &gfv) {
auto [gf, v] = gfv;
return gf->error(continuousValues) + (0.5 * v * v);
};
DecisionTree<Key, double> error_tree(factors_, errorFunc);
return error_tree;
}

14
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -33,6 +33,9 @@ class HybridValues;
class DiscreteValues;
class VectorValues;
/// Alias for pair of GaussianFactor::shared_pointer and a double value.
using GaussianFactorValuePair = std::pair<GaussianFactor::shared_ptr, double>;
/**
* @brief Implementation of a discrete conditional mixture factor.
* Implements a joint discrete-continuous factor where the discrete variable
@ -53,7 +56,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, std::pair<sharedFactor, double>>;
using Factors = DecisionTree<Key, GaussianFactorValuePair>;
private:
/// Decision tree of Gaussian factors indexed by discrete keys.
@ -95,9 +98,9 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
* @param factors Vector of gaussian factor shared pointers
* and arbitrary scalars.
*/
HybridGaussianFactor(
const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
const std::vector<std::pair<sharedFactor, double>> &factors)
HybridGaussianFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
const std::vector<GaussianFactorValuePair> &factors)
: HybridGaussianFactor(continuousKeys, discreteKeys,
Factors(discreteKeys, factors)) {}
@ -115,8 +118,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
/// @{
/// Get the factor and scalar at a given discrete assignment.
std::pair<sharedFactor, double> operator()(
const DiscreteValues &assignment) const;
GaussianFactorValuePair operator()(const DiscreteValues &assignment) const;
/**
* @brief Combine the Gaussian Factor Graphs in `sum` and `this` while

11
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -263,9 +263,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
} else if (auto gmf = dynamic_pointer_cast<HybridGaussianFactor>(f)) {
// Case where we have a HybridGaussianFactor with no continuous keys.
// In this case, compute discrete probabilities.
auto logProbability =
[&](const std::pair<GaussianFactor::shared_ptr, double> &fv)
-> double {
auto logProbability = [&](const GaussianFactorValuePair &fv) -> double {
auto [factor, val] = fv;
double v = 0.5 * val * val;
if (!factor) return -v;
@ -353,8 +351,7 @@ 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) -> std::pair<GaussianFactor::shared_ptr, double> {
auto correct = [&](const Result &pair) -> GaussianFactorValuePair {
const auto &[conditional, factor] = pair;
if (factor) {
auto hf = std::dynamic_pointer_cast<HessianFactor>(factor);
@ -365,8 +362,8 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
}
return {factor, 0.0};
};
DecisionTree<Key, std::pair<GaussianFactor::shared_ptr, double>> newFactors(
eliminationResults, correct);
DecisionTree<Key, GaussianFactorValuePair> newFactors(eliminationResults,
correct);
return std::make_shared<HybridGaussianFactor>(continuousSeparator,
discreteSeparator, newFactors);

2
gtsam/hybrid/HybridNonlinearFactor.h
View File

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