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

Merge branch 'develop' into fix-python-postfix

release/4.3a0
Varun Agrawal 2024-09-27 16:38:51 -04:00
parent 95098e0cad de23ebbd16
commit 11e7ca5247
5 changed files with 201 additions and 212 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
.github/workflows/build-python.yml vendored
View File

@ -138,7 +138,12 @@ jobs:
# Use the prebuilt binary for Windows
$Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE-${{matrix.platform}}.exe"
(New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe")
# Create WebClient with appropriate settings and download Boost exe
$wc = New-Object System.Net.Webclient
$wc.Headers.Add("User-Agent: Other");
$wc.DownloadFile($Url, "$env:TEMP\boost.exe")
Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=$BOOST_PATH"
# Set the BOOST_ROOT variable

10
.github/workflows/build-windows.yml vendored
View File

@ -70,9 +70,6 @@ jobs:
}
if ("${{ matrix.compiler }}" -eq "gcc") {
# Chocolatey GCC is broken on the windows-2019 image.
# See: https://github.com/DaanDeMeyer/doctest/runs/231595515
# See: https://github.community/t5/GitHub-Actions/Something-is-wrong-with-the-chocolatey-installed-version-of-gcc/td-p/32413
scoop install gcc --global
echo "CC=gcc" >> $GITHUB_ENV
echo "CXX=g++" >> $GITHUB_ENV
@ -98,7 +95,12 @@ jobs:
# Use the prebuilt binary for Windows
$Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE-${{matrix.platform}}.exe"
(New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe")
# Create WebClient with appropriate settings and download Boost exe
$wc = New-Object System.Net.Webclient
$wc.Headers.Add("User-Agent: Other");
$wc.DownloadFile($Url, "$env:TEMP\boost.exe")
Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=$BOOST_PATH"
# Set the BOOST_ROOT variable

172
gtsam/hybrid/tests/testGaussianMixture.cpp Normal file
View File

@ -0,0 +1,172 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testGaussianMixture.cpp
* @brief Test hybrid elimination with a simple mixture model
* @author Varun Agrawal
* @author Frank Dellaert
* @date September 2024
*/
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/NoiseModel.h>
// Include for test suite
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
using symbol_shorthand::M;
using symbol_shorthand::Z;
// Define mode key and an assignment m==1
const DiscreteKey m(M(0), 2);
const DiscreteValues m1Assignment{{M(0), 1}};
// Define a 50/50 prior on the mode
DiscreteConditional::shared_ptr mixing =
std::make_shared<DiscreteConditional>(m, "60/40");
// define Continuous keys
const KeyVector continuousKeys{Z(0)};
/**
* Create a simple Gaussian Mixture Model represented as p(z|m)P(m)
* where m is a discrete variable and z is a continuous variable.
* The "mode" m is binary and depending on m, we have 2 different means
* μ1 and μ2 for the Gaussian density p(z|m).
*/
HybridBayesNet GaussianMixtureModel(double mu0, double mu1, double sigma0,
double sigma1) {
HybridBayesNet hbn;
auto model0 = noiseModel::Isotropic::Sigma(1, sigma0);
auto model1 = noiseModel::Isotropic::Sigma(1, sigma1);
auto c0 = std::make_shared<GaussianConditional>(Z(0), Vector1(mu0), I_1x1,
model0),
c1 = std::make_shared<GaussianConditional>(Z(0), Vector1(mu1), I_1x1,
model1);
hbn.emplace_shared<HybridGaussianConditional>(continuousKeys, KeyVector{}, m,
std::vector{c0, c1});
hbn.push_back(mixing);
return hbn;
}
/// Gaussian density function
double Gaussian(double mu, double sigma, double z) {
return exp(-0.5 * pow((z - mu) / sigma, 2)) / sqrt(2 * M_PI * sigma * sigma);
};
/**
* Closed form computation of P(m=1|z).
* If sigma0 == sigma1, it simplifies to a sigmoid function.
* Hardcodes 60/40 prior on mode.
*/
double prob_m_z(double mu0, double mu1, double sigma0, double sigma1,
double z) {
const double p0 = 0.6 * Gaussian(mu0, sigma0, z);
const double p1 = 0.4 * Gaussian(mu1, sigma1, z);
return p1 / (p0 + p1);
};
/// Given \phi(m;z)\phi(m) use eliminate to obtain P(m|z).
DiscreteConditional SolveHFG(const HybridGaussianFactorGraph &hfg) {
return *hfg.eliminateSequential()->at(0)->asDiscrete();
}
/// Given p(z,m) and z, convert to HFG and solve.
DiscreteConditional SolveHBN(const HybridBayesNet &hbn, double z) {
VectorValues given{{Z(0), Vector1(z)}};
return SolveHFG(hbn.toFactorGraph(given));
}
/*
* Test a Gaussian Mixture Model P(m)p(z|m) with same sigma.
* The posterior, as a function of z, should be a sigmoid function.
*/
TEST(GaussianMixture, GaussianMixtureModel) {
double mu0 = 1.0, mu1 = 3.0;
double sigma = 2.0;
auto hbn = GaussianMixtureModel(mu0, mu1, sigma, sigma);
// At the halfway point between the means, we should get P(m|z)=0.5
double midway = mu1 - mu0;
auto pMid = SolveHBN(hbn, midway);
EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid));
// Everywhere else, the result should be a sigmoid.
for (const double shift : {-4, -2, 0, 2, 4}) {
const double z = midway + shift;
const double expected = prob_m_z(mu0, mu1, sigma, sigma, z);
// Workflow 1: convert HBN to HFG and solve
auto posterior1 = SolveHBN(hbn, z);
EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
// Workflow 2: directly specify HFG and solve
HybridGaussianFactorGraph hfg1;
hfg1.emplace_shared<DecisionTreeFactor>(
m, std::vector{Gaussian(mu0, sigma, z), Gaussian(mu1, sigma, z)});
hfg1.push_back(mixing);
auto posterior2 = SolveHFG(hfg1);
EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
}
}
/*
* Test a Gaussian Mixture Model P(m)p(z|m) with different sigmas.
* The posterior, as a function of z, should be a unimodal function.
*/
TEST(GaussianMixture, GaussianMixtureModel2) {
double mu0 = 1.0, mu1 = 3.0;
double sigma0 = 8.0, sigma1 = 4.0;
auto hbn = GaussianMixtureModel(mu0, mu1, sigma0, sigma1);
// We get zMax=3.1333 by finding the maximum value of the function, at which
// point the mode m==1 is about twice as probable as m==0.
double zMax = 3.133;
auto pMax = SolveHBN(hbn, zMax);
EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4));
// Everywhere else, the result should be a bell curve like function.
for (const double shift : {-4, -2, 0, 2, 4}) {
const double z = zMax + shift;
const double expected = prob_m_z(mu0, mu1, sigma0, sigma1, z);
// Workflow 1: convert HBN to HFG and solve
auto posterior1 = SolveHBN(hbn, z);
EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
// Workflow 2: directly specify HFG and solve
HybridGaussianFactorGraph hfg;
hfg.emplace_shared<DecisionTreeFactor>(
m, std::vector{Gaussian(mu0, sigma0, z), Gaussian(mu1, sigma1, z)});
hfg.push_back(mixing);
auto posterior2 = SolveHFG(hfg);
EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

187
gtsam/hybrid/tests/testHybridGaussianFactor.cpp
View File

@ -213,193 +213,6 @@ TEST(HybridGaussianFactor, Error) {
4.0, hybridFactor.error({continuousValues, discreteValues}), 1e-9);
}
namespace test_gmm {
/**
* Function to compute P(m=1|z). For P(m=0|z), swap mus and sigmas.
* If sigma0 == sigma1, it simplifies to a sigmoid function.
*
* Follows equation 7.108 since it is more generic.
*/
double prob_m_z(double mu0, double mu1, double sigma0, double sigma1,
double z) {
double x1 = ((z - mu0) / sigma0), x2 = ((z - mu1) / sigma1);
double d = sigma1 / sigma0;
double e = d * std::exp(-0.5 * (x1 * x1 - x2 * x2));
return 1 / (1 + e);
};
static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1,
double sigma0, double sigma1) {
DiscreteKey m(M(0), 2);
Key z = Z(0);
auto model0 = noiseModel::Isotropic::Sigma(1, sigma0);
auto model1 = noiseModel::Isotropic::Sigma(1, sigma1);
auto c0 = make_shared<GaussianConditional>(z, Vector1(mu0), I_1x1, model0),
c1 = make_shared<GaussianConditional>(z, Vector1(mu1), I_1x1, model1);
HybridBayesNet hbn;
DiscreteKeys discreteParents{m};
hbn.emplace_shared<HybridGaussianConditional>(
KeyVector{z}, KeyVector{}, discreteParents,
HybridGaussianConditional::Conditionals(discreteParents,
std::vector{c0, c1}));
auto mixing = make_shared<DiscreteConditional>(m, "50/50");
hbn.push_back(mixing);
return hbn;
}
} // namespace test_gmm
/* ************************************************************************* */
/**
* Test a simple Gaussian Mixture Model represented as P(m)P(z|m)
* where m is a discrete variable and z is a continuous variable.
* m is binary and depending on m, we have 2 different means
* μ1 and μ2 for the Gaussian distribution around which we sample z.
*
* The resulting factor graph should eliminate to a Bayes net
* which represents a sigmoid function.
*/
TEST(HybridGaussianFactor, GaussianMixtureModel) {
using namespace test_gmm;
double mu0 = 1.0, mu1 = 3.0;
double sigma = 2.0;
DiscreteKey m(M(0), 2);
Key z = Z(0);
auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma, sigma);
// The result should be a sigmoid.
// So should be P(m=1|z) = 0.5 at z=3.0 - 1.0=2.0
double midway = mu1 - mu0, lambda = 4;
{
VectorValues given;
given.insert(z, Vector1(midway));
HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
EXPECT_DOUBLES_EQUAL(
prob_m_z(mu0, mu1, sigma, sigma, midway),
bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}),
1e-8);
// At the halfway point between the means, we should get P(m|z)=0.5
HybridBayesNet expected;
expected.emplace_shared<DiscreteConditional>(m, "50/50");
EXPECT(assert_equal(expected, *bn));
}
{
// Shift by -lambda
VectorValues given;
given.insert(z, Vector1(midway - lambda));
HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
EXPECT_DOUBLES_EQUAL(
prob_m_z(mu0, mu1, sigma, sigma, midway - lambda),
bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}),
1e-8);
}
{
// Shift by lambda
VectorValues given;
given.insert(z, Vector1(midway + lambda));
HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
EXPECT_DOUBLES_EQUAL(
prob_m_z(mu0, mu1, sigma, sigma, midway + lambda),
bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}),
1e-8);
}
}
/* ************************************************************************* */
/**
* Test a simple Gaussian Mixture Model represented as P(m)P(z|m)
* where m is a discrete variable and z is a continuous variable.
* m is binary and depending on m, we have 2 different means
* and covariances each for the
* Gaussian distribution around which we sample z.
*
* The resulting factor graph should eliminate to a Bayes net
* which represents a Gaussian-like function
* where m1>m0 close to 3.1333.
*/
TEST(HybridGaussianFactor, GaussianMixtureModel2) {
using namespace test_gmm;
double mu0 = 1.0, mu1 = 3.0;
double sigma0 = 8.0, sigma1 = 4.0;
DiscreteKey m(M(0), 2);
Key z = Z(0);
auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma0, sigma1);
double m1_high = 3.133, lambda = 4;
{
// The result should be a bell curve like function
// with m1 > m0 close to 3.1333.
// We get 3.1333 by finding the maximum value of the function.
VectorValues given;
given.insert(z, Vector1(3.133));
HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
EXPECT_DOUBLES_EQUAL(
prob_m_z(mu0, mu1, sigma0, sigma1, m1_high),
bn->at(0)->asDiscrete()->operator()(DiscreteValues{{M(0), 1}}), 1e-8);
// At the halfway point between the means
HybridBayesNet expected;
expected.emplace_shared<DiscreteConditional>(
m, DiscreteKeys{},
vector<double>{prob_m_z(mu1, mu0, sigma1, sigma0, m1_high),
prob_m_z(mu0, mu1, sigma0, sigma1, m1_high)});
EXPECT(assert_equal(expected, *bn));
}
{
// Shift by -lambda
VectorValues given;
given.insert(z, Vector1(m1_high - lambda));
HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
EXPECT_DOUBLES_EQUAL(
prob_m_z(mu0, mu1, sigma0, sigma1, m1_high - lambda),
bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}),
1e-8);
}
{
// Shift by lambda
VectorValues given;
given.insert(z, Vector1(m1_high + lambda));
HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
HybridBayesNet::shared_ptr bn = gfg.eliminateSequential();
EXPECT_DOUBLES_EQUAL(
prob_m_z(mu0, mu1, sigma0, sigma1, m1_high + lambda),
bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}),
1e-8);
}
}
namespace test_two_state_estimation {
DiscreteKey m1(M(1), 2);

37
gtsam/linear/NoiseModel.cpp
View File

@ -269,10 +269,7 @@ double Gaussian::negLogConstant() const {
/* ************************************************************************* */
// Diagonal
/* ************************************************************************* */
Diagonal::Diagonal() :
Gaussian(1) // TODO: Frank asks: really sure about this?
{
}
Diagonal::Diagonal() : Gaussian() {}
/* ************************************************************************* */
Diagonal::Diagonal(const Vector& sigmas)
@ -284,31 +281,30 @@ Diagonal::Diagonal(const Vector& sigmas)
/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
if (smart) {
// check whether all the same entry
size_t n = variances.size();
for (size_t j = 1; j < n; j++)
if (variances(j) != variances(0)) goto full;
return Isotropic::Variance(n, variances(0), true);
}
full: return shared_ptr(new Diagonal(variances.cwiseSqrt()));
// check whether all the same entry
return (smart && (variances.array() == variances(0)).all())
? Isotropic::Variance(variances.size(), variances(0), true)
: shared_ptr(new Diagonal(variances.cwiseSqrt()));
}
/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
if (smart) {
size_t n = sigmas.size();
if (n==0) goto full;
if (n == 0) goto full;
// look for zeros to make a constraint
for (size_t j=0; j< n; ++j)
if (sigmas(j)<1e-8)
return Constrained::MixedSigmas(sigmas);
if ((sigmas.array() < 1e-8).any()) {
return Constrained::MixedSigmas(sigmas);
}
// check whether all the same entry
for (size_t j = 1; j < n; j++)
if (sigmas(j) != sigmas(0)) goto full;
return Isotropic::Sigma(n, sigmas(0), true);
if ((sigmas.array() == sigmas(0)).all()) {
return Isotropic::Sigma(n, sigmas(0), true);
}
}
full: return Diagonal::shared_ptr(new Diagonal(sigmas));
full:
return Diagonal::shared_ptr(new Diagonal(sigmas));
}
/* ************************************************************************* */
@ -316,6 +312,7 @@ Diagonal::shared_ptr Diagonal::Precisions(const Vector& precisions,
bool smart) {
return Variances(precisions.array().inverse(), smart);
}
/* ************************************************************************* */
void Diagonal::print(const string& name) const {
gtsam::print(sigmas_, name + "diagonal sigmas ");