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

Store negLogConstant[i] - negLogConstant_

release/4.3a0
Frank Dellaert 2024-10-08 16:14:56 +09:00
parent f0770c26cf
commit bcd94e32a3
2 changed files with 66 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -33,6 +33,15 @@
namespace gtsam { namespace gtsam {
/* *******************************************************************************/ /* *******************************************************************************/
/**
* @brief Helper struct for constructing HybridGaussianConditional objects
*
* This struct contains the following fields:
* - nrFrontals: Optional size_t for number of frontal variables
* - pairs: FactorValuePairs for storing conditionals with their negLogConstant
* - conditionals: Conditionals for storing conditionals. TODO(frank): kill!
* - minNegLogConstant: minimum negLogConstant, computed here, subtracted in constructor
*/
struct HybridGaussianConditional::Helper { struct HybridGaussianConditional::Helper {
std::optional<size_t> nrFrontals; std::optional<size_t> nrFrontals;
FactorValuePairs pairs; FactorValuePairs pairs;
@ -67,16 +76,12 @@ struct HybridGaussianConditional::Helper {
/// Construct from tree of GaussianConditionals. /// Construct from tree of GaussianConditionals.
explicit Helper(const Conditionals& conditionals) explicit Helper(const Conditionals& conditionals)
: conditionals(conditionals), minNegLogConstant(std::numeric_limits<double>::infinity()) { : conditionals(conditionals), minNegLogConstant(std::numeric_limits<double>::infinity()) {
auto func = [this](const GC::shared_ptr& c) -> GaussianFactorValuePair { auto func = [this](const GC::shared_ptr& gc) -> GaussianFactorValuePair {
double value = 0.0; if (!gc) return {nullptr, std::numeric_limits<double>::infinity()};
if (c) { if (!nrFrontals) nrFrontals = gc->nrFrontals();
if (!nrFrontals.has_value()) { double value = gc->negLogConstant();
nrFrontals = c->nrFrontals(); minNegLogConstant = std::min(minNegLogConstant, value);
} return {gc, value};
value = c->negLogConstant();
minNegLogConstant = std::min(minNegLogConstant, value);
}
return {std::dynamic_pointer_cast<GaussianFactor>(c), value};
}; };
pairs = FactorValuePairs(conditionals, func); pairs = FactorValuePairs(conditionals, func);
if (!nrFrontals.has_value()) { if (!nrFrontals.has_value()) {
@ -90,7 +95,13 @@ struct HybridGaussianConditional::Helper {
/* *******************************************************************************/ /* *******************************************************************************/
HybridGaussianConditional::HybridGaussianConditional(const DiscreteKeys& discreteParents, HybridGaussianConditional::HybridGaussianConditional(const DiscreteKeys& discreteParents,
const Helper& helper) const Helper& helper)
: BaseFactor(discreteParents, helper.pairs), : BaseFactor(
discreteParents,
FactorValuePairs(
helper.pairs,
[&](const GaussianFactorValuePair& pair) { // subtract minNegLogConstant
return GaussianFactorValuePair{pair.first, pair.second - helper.minNegLogConstant};
})),
BaseConditional(*helper.nrFrontals), BaseConditional(*helper.nrFrontals),
conditionals_(helper.conditionals), conditionals_(helper.conditionals),
negLogConstant_(helper.minNegLogConstant) {} negLogConstant_(helper.minNegLogConstant) {}

106
gtsam/hybrid/tests/testHybridGaussianConditional.cpp
View File

@ -18,6 +18,8 @@
* @date December 2021 * @date December 2021
*/ */
#include <gtsam/discrete/DecisionTree.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteValues.h> #include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridGaussianConditional.h> #include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.h> #include <gtsam/hybrid/HybridGaussianFactor.h>
@ -28,9 +30,6 @@
#include <memory> #include <memory>
#include <vector> #include <vector>
#include "gtsam/discrete/DecisionTree.h"
#include "gtsam/discrete/DiscreteKey.h"
// Include for test suite // Include for test suite
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -52,10 +51,8 @@ namespace equal_constants {
// Create a simple HybridGaussianConditional // Create a simple HybridGaussianConditional
const double commonSigma = 2.0; const double commonSigma = 2.0;
const std::vector<GaussianConditional::shared_ptr> conditionals{ const std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), commonSigma),
commonSigma), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), commonSigma)};
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
commonSigma)};
const HybridGaussianConditional hybrid_conditional(mode, conditionals); const HybridGaussianConditional hybrid_conditional(mode, conditionals);
} // namespace equal_constants } // namespace equal_constants
@ -80,8 +77,8 @@ TEST(HybridGaussianConditional, LogProbability) {
using namespace equal_constants; using namespace equal_constants;
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
EXPECT_DOUBLES_EQUAL(conditionals[mode]->logProbability(vv), EXPECT_DOUBLES_EQUAL(
hybrid_conditional.logProbability(hv), 1e-8); conditionals[mode]->logProbability(vv), hybrid_conditional.logProbability(hv), 1e-8);
} }
} }
@ -93,8 +90,7 @@ TEST(HybridGaussianConditional, Error) {
// Check result. // Check result.
DiscreteKeys discrete_keys{mode}; DiscreteKeys discrete_keys{mode};
std::vector<double> leaves = {conditionals[0]->error(vv), std::vector<double> leaves = {conditionals[0]->error(vv), conditionals[1]->error(vv)};
conditionals[1]->error(vv)};
AlgebraicDecisionTree<Key> expected(discrete_keys, leaves); AlgebraicDecisionTree<Key> expected(discrete_keys, leaves);
EXPECT(assert_equal(expected, actual, 1e-6)); EXPECT(assert_equal(expected, actual, 1e-6));
@ -102,8 +98,7 @@ TEST(HybridGaussianConditional, Error) {
// Check for non-tree version. // Check for non-tree version.
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
EXPECT_DOUBLES_EQUAL(conditionals[mode]->error(vv), EXPECT_DOUBLES_EQUAL(conditionals[mode]->error(vv), hybrid_conditional.error(hv), 1e-8);
hybrid_conditional.error(hv), 1e-8);
} }
} }
@ -118,10 +113,8 @@ TEST(HybridGaussianConditional, Likelihood) {
// Check that the hybrid conditional error and the likelihood error are the // Check that the hybrid conditional error and the likelihood error are the
// same. // same.
EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv0), likelihood->error(hv0), EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv0), likelihood->error(hv0), 1e-8);
1e-8); EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), likelihood->error(hv1), 1e-8);
EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), likelihood->error(hv1),
1e-8);
// Check that likelihood error is as expected, i.e., just the errors of the // Check that likelihood error is as expected, i.e., just the errors of the
// individual likelihoods, in the `equal_constants` case. // individual likelihoods, in the `equal_constants` case.
@ -135,8 +128,7 @@ TEST(HybridGaussianConditional, Likelihood) {
std::vector<double> ratio(2); std::vector<double> ratio(2);
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
ratio[mode] = ratio[mode] = std::exp(-likelihood->error(hv)) / hybrid_conditional.evaluate(hv);
std::exp(-likelihood->error(hv)) / hybrid_conditional.evaluate(hv);
} }
EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8); EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8);
} }
@ -146,10 +138,8 @@ namespace mode_dependent_constants {
// Create a HybridGaussianConditional with mode-dependent noise models. // Create a HybridGaussianConditional with mode-dependent noise models.
// 0 is low-noise, 1 is high-noise. // 0 is low-noise, 1 is high-noise.
const std::vector<GaussianConditional::shared_ptr> conditionals{ const std::vector<GaussianConditional::shared_ptr> conditionals{
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), 0.5),
0.5), GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), 3.0)};
GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0),
3.0)};
const HybridGaussianConditional hybrid_conditional(mode, conditionals); const HybridGaussianConditional hybrid_conditional(mode, conditionals);
} // namespace mode_dependent_constants } // namespace mode_dependent_constants
@ -181,9 +171,8 @@ TEST(HybridGaussianConditional, Error2) {
// Expected error is e(X) + log(sqrt(|2πΣ|)). // Expected error is e(X) + log(sqrt(|2πΣ|)).
// We normalize log(sqrt(|2πΣ|)) with min(negLogConstant) // We normalize log(sqrt(|2πΣ|)) with min(negLogConstant)
// so it is non-negative. // so it is non-negative.
std::vector<double> leaves = { std::vector<double> leaves = {conditionals[0]->error(vv) + negLogConstant0 - minErrorConstant,
conditionals[0]->error(vv) + negLogConstant0 - minErrorConstant, conditionals[1]->error(vv) + negLogConstant1 - minErrorConstant};
conditionals[1]->error(vv) + negLogConstant1 - minErrorConstant};
AlgebraicDecisionTree<Key> expected(discrete_keys, leaves); AlgebraicDecisionTree<Key> expected(discrete_keys, leaves);
EXPECT(assert_equal(expected, actual, 1e-6)); EXPECT(assert_equal(expected, actual, 1e-6));
@ -191,10 +180,10 @@ TEST(HybridGaussianConditional, Error2) {
// Check for non-tree version. // Check for non-tree version.
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
EXPECT_DOUBLES_EQUAL(conditionals[mode]->error(vv) + EXPECT_DOUBLES_EQUAL(
conditionals[mode]->negLogConstant() - conditionals[mode]->error(vv) + conditionals[mode]->negLogConstant() - minErrorConstant,
minErrorConstant, hybrid_conditional.error(hv),
hybrid_conditional.error(hv), 1e-8); 1e-8);
} }
} }
@ -209,10 +198,8 @@ TEST(HybridGaussianConditional, Likelihood2) {
// Check that the hybrid conditional error and the likelihood error are as // Check that the hybrid conditional error and the likelihood error are as
// expected, this invariant is the same as the equal noise case: // expected, this invariant is the same as the equal noise case:
EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv0), likelihood->error(hv0), EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv0), likelihood->error(hv0), 1e-8);
1e-8); EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), likelihood->error(hv1), 1e-8);
EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), likelihood->error(hv1),
1e-8);
// Check the detailed JacobianFactor calculation for mode==1. // Check the detailed JacobianFactor calculation for mode==1.
{ {
@ -221,34 +208,18 @@ TEST(HybridGaussianConditional, Likelihood2) {
const auto jf1 = std::dynamic_pointer_cast<JacobianFactor>(gf1); const auto jf1 = std::dynamic_pointer_cast<JacobianFactor>(gf1);
CHECK(jf1); CHECK(jf1);
// It has 2 rows, not 1! // Check that the JacobianFactor error with constants is equal to the conditional error:
CHECK(jf1->rows() == 2); EXPECT_DOUBLES_EQUAL(
hybrid_conditional.error(hv1),
// Check that the constant C1 is properly encoded in the JacobianFactor. jf1->error(hv1) + conditionals[1]->negLogConstant() - hybrid_conditional.negLogConstant(),
const double C1 = 1e-8);
conditionals[1]->negLogConstant() - hybrid_conditional.negLogConstant();
const double c1 = std::sqrt(2.0 * C1);
Vector expected_unwhitened(2);
expected_unwhitened << 4.9 - 5.0, -c1;
Vector actual_unwhitened = jf1->unweighted_error(vv);
EXPECT(assert_equal(expected_unwhitened, actual_unwhitened));
// Make sure the noise model does not touch it.
Vector expected_whitened(2);
expected_whitened << (4.9 - 5.0) / 3.0, -c1;
Vector actual_whitened = jf1->error_vector(vv);
EXPECT(assert_equal(expected_whitened, actual_whitened));
// Check that the error is equal to the conditional error:
EXPECT_DOUBLES_EQUAL(hybrid_conditional.error(hv1), jf1->error(hv1), 1e-8);
} }
// Check that the ratio of probPrime to evaluate is the same for all modes. // Check that the ratio of probPrime to evaluate is the same for all modes.
std::vector<double> ratio(2); std::vector<double> ratio(2);
for (size_t mode : {0, 1}) { for (size_t mode : {0, 1}) {
const HybridValues hv{vv, {{M(0), mode}}}; const HybridValues hv{vv, {{M(0), mode}}};
ratio[mode] = ratio[mode] = std::exp(-likelihood->error(hv)) / hybrid_conditional.evaluate(hv);
std::exp(-likelihood->error(hv)) / hybrid_conditional.evaluate(hv);
} }
EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8); EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8);
} }
@ -261,8 +232,7 @@ TEST(HybridGaussianConditional, Prune) {
DiscreteKeys modes{{M(1), 2}, {M(2), 2}}; DiscreteKeys modes{{M(1), 2}, {M(2), 2}};
std::vector<GaussianConditional::shared_ptr> gcs; std::vector<GaussianConditional::shared_ptr> gcs;
for (size_t i = 0; i < 4; i++) { for (size_t i = 0; i < 4; i++) {
gcs.push_back( gcs.push_back(GaussianConditional::sharedMeanAndStddev(Z(0), Vector1(i + 1), i + 1));
GaussianConditional::sharedMeanAndStddev(Z(0), Vector1(i + 1), i + 1));
} }
auto empty = std::make_shared<GaussianConditional>(); auto empty = std::make_shared<GaussianConditional>();
HybridGaussianConditional::Conditionals conditionals(modes, gcs); HybridGaussianConditional::Conditionals conditionals(modes, gcs);
@ -282,8 +252,14 @@ TEST(HybridGaussianConditional, Prune) {
} }
} }
{ {
const std::vector<double> potentials{0, 0, 0.5, 0, // const std::vector<double> potentials{0,
0, 0, 0.5, 0}; 0,
0.5,
0, //
0,
0,
0.5,
0};
const DecisionTreeFactor decisionTreeFactor(keys, potentials); const DecisionTreeFactor decisionTreeFactor(keys, potentials);
const auto pruned = hgc.prune(decisionTreeFactor); const auto pruned = hgc.prune(decisionTreeFactor);
@ -292,8 +268,14 @@ TEST(HybridGaussianConditional, Prune) {
EXPECT_LONGS_EQUAL(2, pruned->nrComponents()); EXPECT_LONGS_EQUAL(2, pruned->nrComponents());
} }
{ {
const std::vector<double> potentials{0.2, 0, 0.3, 0, // const std::vector<double> potentials{0.2,
0, 0, 0.5, 0}; 0,
0.3,
0, //
0,
0,
0.5,
0};
const DecisionTreeFactor decisionTreeFactor(keys, potentials); const DecisionTreeFactor decisionTreeFactor(keys, potentials);
const auto pruned = hgc.prune(decisionTreeFactor); const auto pruned = hgc.prune(decisionTreeFactor);