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

minor refactoring

release/4.3a0
Varun Agrawal 2022-12-21 20:20:56 +05:30
parent 5da56c1393
commit a27979e84b
2 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -521,6 +521,7 @@ double HybridGaussianFactorGraph::probPrime(
const VectorValues &continuousValues, const VectorValues &continuousValues,
const DiscreteValues &discreteValues) const { const DiscreteValues &discreteValues) const {
double error = this->error(continuousValues, discreteValues); double error = this->error(continuousValues, discreteValues);
// NOTE: The 0.5 term is handled by each factor
return std::exp(-error); return std::exp(-error);
} }
@ -528,8 +529,10 @@ double HybridGaussianFactorGraph::probPrime(
AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::probPrime( AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::probPrime(
const VectorValues &continuousValues) const { const VectorValues &continuousValues) const {
AlgebraicDecisionTree<Key> error_tree = this->error(continuousValues); AlgebraicDecisionTree<Key> error_tree = this->error(continuousValues);
AlgebraicDecisionTree<Key> prob_tree = AlgebraicDecisionTree<Key> prob_tree = error_tree.apply([](double error) {
error_tree.apply([](double error) { return exp(-error); }); // NOTE: The 0.5 term is handled by each factor
return exp(-error);
});
return prob_tree; return prob_tree;
} }

11
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -12,7 +12,7 @@
/** /**
* @file HybridGaussianFactorGraph.h * @file HybridGaussianFactorGraph.h
* @brief Linearized Hybrid factor graph that uses type erasure * @brief Linearized Hybrid factor graph that uses type erasure
* @author Fan Jiang * @author Fan Jiang, Varun Agrawal
* @date Mar 11, 2022 * @date Mar 11, 2022
*/ */
@ -270,10 +270,9 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
const std::vector<DiscreteValues> assignments = const std::vector<DiscreteValues> assignments =
DiscreteValues::CartesianProduct(discrete_keys); DiscreteValues::CartesianProduct(discrete_keys);
// Save a copy of the original discrete key ordering // Get reversed discrete key ordering for correct tree construction
DiscreteKeys reversed_discrete_keys(discrete_keys); DiscreteKeys reversed_discrete_keys(discrete_keys.rbegin(),
// Reverse discrete keys order for correct tree construction discrete_keys.rend());
std::reverse(reversed_discrete_keys.begin(), reversed_discrete_keys.end());
// Create a decision tree of all the different VectorValues // Create a decision tree of all the different VectorValues
DecisionTree<Key, VectorValues::shared_ptr> delta_tree = DecisionTree<Key, VectorValues::shared_ptr> delta_tree =
@ -286,7 +285,7 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
VectorValues delta = *delta_tree(assignment); VectorValues delta = *delta_tree(assignment);
// If VectorValues is empty, it means this is a pruned branch. // If VectorValues is empty, it means this is a pruned branch.
// Set thr probPrime to 0.0. // Set the probPrime to 0.0.
if (delta.size() == 0) { if (delta.size() == 0) {
probPrimes.push_back(0.0); probPrimes.push_back(0.0);
continue; continue;