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

clean up code

release/4.3a0
Varun Agrawal 2024-09-04 15:18:50 -04:00
parent cc04003716
commit 8f65f058e0
4 changed files with 10 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/hybrid/HybridFactorGraph.cpp
View File

@ -49,15 +49,6 @@ KeySet HybridFactorGraph::discreteKeySet() const {
return keys;
}
/* ************************************************************************* */
std::unordered_map<Key, DiscreteKey> HybridFactorGraph::discreteKeyMap() const {
std::unordered_map<Key, DiscreteKey> result;
for (const DiscreteKey& k : discreteKeys()) {
result[k.first] = k;
}
return result;
}
/* ************************************************************************* */
const KeySet HybridFactorGraph::continuousKeySet() const {
KeySet keys;

3
gtsam/hybrid/HybridFactorGraph.h
View File

@ -69,9 +69,6 @@ class GTSAM_EXPORT HybridFactorGraph : public FactorGraph<Factor> {
/// Get all the discrete keys in the factor graph, as a set of Keys.
KeySet discreteKeySet() const;
/// Get a map from Key to corresponding DiscreteKey.
std::unordered_map<Key, DiscreteKey> discreteKeyMap() const;
/// Get all the continuous keys in the factor graph.
const KeySet continuousKeySet() const;

20
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -312,8 +312,8 @@ 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 μ.
* Compute the probability p(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m)
* from the residual error ||b||^2 at the mean μ.
* The residual error contains no keys, and only
* depends on the discrete separator if present.
*/
@ -523,19 +523,9 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
std::inserter(continuousSeparator, continuousSeparator.begin()));
// Similarly for the discrete separator.
auto discreteKeySet = factors.discreteKeySet();
// Use set-difference to get the difference in keys
KeySet discreteSeparatorSet;
std::set_difference(
discreteKeySet.begin(), discreteKeySet.end(), frontalKeysSet.begin(),
frontalKeysSet.end(),
std::inserter(discreteSeparatorSet, discreteSeparatorSet.begin()));
// Convert from set of keys to set of DiscreteKeys
std::set<DiscreteKey> discreteSeparator;
auto discreteKeyMap = factors.discreteKeyMap();
for (auto key : discreteSeparatorSet) {
discreteSeparator.insert(discreteKeyMap.at(key));
}
// Since we eliminate all continuous variables first,
// the discrete separator will be *all* the discrete keys.
std::set<DiscreteKey> discreteSeparator = factors.discreteKeys();
return hybridElimination(factors, frontalKeys, continuousSeparator,
discreteSeparator);

8
gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
View File

@ -434,12 +434,12 @@ static HybridBayesNet CreateBayesNet(double mu0, double mu1, double sigma0,
/* ************************************************************************* */
/**
* Test a model P(x0)P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
* Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
*
* P(f01|x1,x0,m1) has different means and same covariance.
*
* Converting to a factor graph gives us
* P(x0)ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
* ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
*
* If we only have a measurement on z0, then
* the probability of m1 should be 0.5/0.5.
@ -488,12 +488,12 @@ TEST(GaussianMixtureFactor, TwoStateModel) {
/* ************************************************************************* */
/**
* Test a model P(x0)P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
* Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).
*
* P(f01|x1,x0,m1) has different means and different covariances.
*
* Converting to a factor graph gives us
* P(x0)ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
* ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)
*
* If we only have a measurement on z0, then
* the P(m1) should be 0.5/0.5.