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

Merge branch 'working-hybrid' into direct-hybrid-fg

release/4.3a0
Varun Agrawal 2024-08-22 20:21:18 -04:00
parent 665d755448 91ccbebf22
commit 03e61f459d
6 changed files with 75 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -593,6 +593,55 @@ TEST(ADT, zero) {
EXPECT_DOUBLES_EQUAL(0, anotb(x11), 1e-9);
}
/// Example ADT from 0 to 11.
ADT exampleADT() {
DiscreteKey A(0, 2), B(1, 3), C(2, 2);
ADT f(A & B & C, "0 6 2 8 4 10 1 7 3 9 5 11");
return f;
}
/* ************************************************************************** */
// Test sum
TEST(ADT, Sum) {
ADT a = exampleADT();
double expected_sum = 0;
for (double i = 0; i < 12; i++) {
expected_sum += i;
}
EXPECT_DOUBLES_EQUAL(expected_sum, a.sum(), 1e-9);
}
/* ************************************************************************** */
// Test normalize
TEST(ADT, Normalize) {
ADT a = exampleADT();
double sum = a.sum();
auto actual = a.normalize(sum);
DiscreteKey A(0, 2), B(1, 3), C(2, 2);
DiscreteKeys keys = DiscreteKeys{A, B, C};
std::vector<double> cpt{0 / sum, 6 / sum, 2 / sum, 8 / sum,
4 / sum, 10 / sum, 1 / sum, 7 / sum,
3 / sum, 9 / sum, 5 / sum, 11 / sum};
ADT expected(keys, cpt);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************** */
// Test min
TEST(ADT, Min) {
ADT a = exampleADT();
double min = a.min();
EXPECT_DOUBLES_EQUAL(0.0, min, 1e-9);
}
/* ************************************************************************** */
// Test max
TEST(ADT, Max) {
ADT a = exampleADT();
double max = a.max();
EXPECT_DOUBLES_EQUAL(11.0, max, 1e-9);
}
/* ************************************************************************* */
int main() {
TestResult tr;

2
gtsam/geometry/geometry.i
View File

@ -856,7 +856,7 @@ class Cal3_S2Stereo {
gtsam::Matrix K() const;
gtsam::Point2 principalPoint() const;
double baseline() const;
Vector6 vector() const;
gtsam::Vector6 vector() const;
gtsam::Matrix inverse() const;
};

33
gtsam/hybrid/GaussianMixture.cpp
View File

@ -200,27 +200,24 @@ std::shared_ptr<GaussianMixtureFactor> GaussianMixture::likelihood(
const GaussianMixtureFactor::Factors likelihoods(
conditionals_, [&](const GaussianConditional::shared_ptr &conditional) {
const auto likelihood_m = conditional->likelihood(given);
const double Cgm_Kgcm =
logConstant_ - conditional->logNormalizationConstant();
if (Cgm_Kgcm == 0.0) {
return likelihood_m;
});
// First compute all the sqrt(|2 pi Sigma|) terms
auto computeLogNormalizers = [](const GaussianFactor::shared_ptr &gf) {
auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
// If we have, say, a Hessian factor, then no need to do anything
if (!jf) return 0.0;
auto model = jf->get_model();
// If there is no noise model, there is nothing to do.
if (!model) {
return 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);
}
return ComputeLogNormalizer(model);
};
AlgebraicDecisionTree<Key> log_normalizers =
DecisionTree<Key, double>(likelihoods, computeLogNormalizers);
});
return std::make_shared<GaussianMixtureFactor>(
continuousParentKeys, discreteParentKeys, likelihoods, log_normalizers);
continuousParentKeys, discreteParentKeys, likelihoods);
}
/* ************************************************************************* */

68
gtsam/hybrid/GaussianMixtureFactor.cpp
View File

@ -28,55 +28,11 @@
namespace gtsam {
/**
* @brief Helper function to augment the [A|b] matrices in the factor components
* with the normalizer values.
* This is done by storing the normalizer value in
* the `b` vector as an additional row.
*
* @param factors DecisionTree of GaussianFactor shared pointers.
* @param logNormalizers Tree of log-normalizers corresponding to each
* Gaussian factor in factors.
* @return GaussianMixtureFactor::Factors
*/
GaussianMixtureFactor::Factors augment(
const GaussianMixtureFactor::Factors &factors,
const AlgebraicDecisionTree<Key> &logNormalizers) {
// Find the minimum value so we can "proselytize" to positive values.
// Done because we can't have sqrt of negative numbers.
double min_log_normalizer = logNormalizers.min();
AlgebraicDecisionTree<Key> log_normalizers = logNormalizers.apply(
[&min_log_normalizer](double n) { return n - min_log_normalizer; });
// Finally, update the [A|b] matrices.
auto update = [&log_normalizers](
const Assignment<Key> &assignment,
const GaussianMixtureFactor::sharedFactor &gf) {
auto jf = std::dynamic_pointer_cast<JacobianFactor>(gf);
if (!jf) return gf;
// If the log_normalizer is 0, do nothing
if (log_normalizers(assignment) == 0.0) return gf;
GaussianFactorGraph gfg;
gfg.push_back(jf);
Vector c(1);
c << std::sqrt(log_normalizers(assignment));
auto constantFactor = std::make_shared<JacobianFactor>(c);
gfg.push_back(constantFactor);
return std::dynamic_pointer_cast<GaussianFactor>(
std::make_shared<JacobianFactor>(gfg));
};
return factors.apply(update);
}
/* *******************************************************************************/
GaussianMixtureFactor::GaussianMixtureFactor(
const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
const Factors &factors, const AlgebraicDecisionTree<Key> &logNormalizers)
: Base(continuousKeys, discreteKeys),
factors_(augment(factors, logNormalizers)) {}
GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
const Factors &factors)
: Base(continuousKeys, discreteKeys), factors_(factors) {}
/* *******************************************************************************/
bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
@ -164,20 +120,4 @@ double GaussianMixtureFactor::error(const HybridValues &values) const {
return gf->error(values.continuous());
}
/* *******************************************************************************/
double ComputeLogNormalizer(
const noiseModel::Gaussian::shared_ptr &noise_model) {
// Since noise models are Gaussian, we can get the logDeterminant using
// the same trick as in GaussianConditional
double logDetR = noise_model->R()
.diagonal()
.unaryExpr([](double x) { return log(x); })
.sum();
double logDeterminantSigma = -2.0 * logDetR;
size_t n = noise_model->dim();
constexpr double log2pi = 1.8378770664093454835606594728112;
return n * log2pi + logDeterminantSigma;
}
} // namespace gtsam

14
gtsam/hybrid/GaussianMixtureFactor.h
View File

@ -82,14 +82,10 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
* their cardinalities.
* @param factors The decision tree of Gaussian factors stored as the mixture
* density.
* @param logNormalizers Tree of log-normalizers corresponding to each
* Gaussian factor in factors.
*/
GaussianMixtureFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
const Factors &factors,
const AlgebraicDecisionTree<Key> &logNormalizers =
AlgebraicDecisionTree<Key>(0.0));
const Factors &factors);
/**
* @brief Construct a new GaussianMixtureFactor object using a vector of
@ -98,16 +94,12 @@ class GTSAM_EXPORT GaussianMixtureFactor : 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 logNormalizers Tree of log-normalizers corresponding to each
* Gaussian factor in factors.
*/
GaussianMixtureFactor(const KeyVector &continuousKeys,
const DiscreteKeys &discreteKeys,
const std::vector<sharedFactor> &factors,
const AlgebraicDecisionTree<Key> &logNormalizers =
AlgebraicDecisionTree<Key>(0.0))
const std::vector<sharedFactor> &factors)
: GaussianMixtureFactor(continuousKeys, discreteKeys,
Factors(discreteKeys, factors), logNormalizers) {}
Factors(discreteKeys, factors)) {}
/// @}
/// @name Testable

2
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -80,6 +80,8 @@ TEST(GaussianBayesNet, Evaluate1) {
smallBayesNet.at(0)->logNormalizationConstant() +
smallBayesNet.at(1)->logNormalizationConstant(),
1e-9);
EXPECT_DOUBLES_EQUAL(log(constant), smallBayesNet.logNormalizationConstant(),
1e-9);
const double actual = smallBayesNet.evaluate(mean);
EXPECT_DOUBLES_EQUAL(constant, actual, 1e-9);
}