Merge pull request #1091 from borglab/feature/linear_samplers
Frank Dellaert
GitHub
commit
c34456268a
|
|
@ -26,6 +26,9 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// In Wrappers we have no access to this so have a default ready
|
||||
static std::mt19937_64 kRandomNumberGenerator(42);
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Instantiate base class
|
||||
|
|
@ -37,28 +40,50 @@ namespace gtsam {
|
|||
return Base::equals(bn, tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues GaussianBayesNet::optimize() const
|
||||
{
|
||||
VectorValues soln; // no missing variables -> just create an empty vector
|
||||
return optimize(soln);
|
||||
/* ************************************************************************ */
|
||||
VectorValues GaussianBayesNet::optimize() const {
|
||||
VectorValues solution; // no missing variables -> create an empty vector
|
||||
return optimize(solution);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues GaussianBayesNet::optimize(
|
||||
const VectorValues& solutionForMissing) const {
|
||||
VectorValues soln(solutionForMissing); // possibly empty
|
||||
VectorValues GaussianBayesNet::optimize(VectorValues solution) const {
|
||||
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
|
||||
/** solve each node in turn in topological sort order (parents first)*/
|
||||
for (auto cg: boost::adaptors::reverse(*this)) {
|
||||
// solve each node in reverse topological sort order (parents first)
|
||||
for (auto cg : boost::adaptors::reverse(*this)) {
|
||||
// i^th part of R*x=y, x=inv(R)*y
|
||||
// (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
|
||||
soln.insert(cg->solve(soln));
|
||||
// (Rii*xi + R_i*x(i+1:))./si = yi =>
|
||||
// xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
|
||||
solution.insert(cg->solve(solution));
|
||||
}
|
||||
return soln;
|
||||
return solution;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* ************************************************************************ */
|
||||
VectorValues GaussianBayesNet::sample(std::mt19937_64* rng) const {
|
||||
VectorValues result; // no missing variables -> create an empty vector
|
||||
return sample(result, rng);
|
||||
}
|
||||
|
||||
VectorValues GaussianBayesNet::sample(VectorValues result,
|
||||
std::mt19937_64* rng) const {
|
||||
// sample each node in reverse topological sort order (parents first)
|
||||
for (auto cg : boost::adaptors::reverse(*this)) {
|
||||
const VectorValues sampled = cg->sample(result, rng);
|
||||
result.insert(sampled);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
VectorValues GaussianBayesNet::sample() const {
|
||||
return sample(&kRandomNumberGenerator);
|
||||
}
|
||||
|
||||
VectorValues GaussianBayesNet::sample(VectorValues given) const {
|
||||
return sample(given, &kRandomNumberGenerator);
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
VectorValues GaussianBayesNet::optimizeGradientSearch() const
|
||||
{
|
||||
gttic(GaussianBayesTree_optimizeGradientSearch);
|
||||
|
|
|
|||
|
|
@ -88,11 +88,35 @@ namespace gtsam {
|
|||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by back-substitution
|
||||
/// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by
|
||||
/// back-substitution
|
||||
VectorValues optimize() const;
|
||||
|
||||
/// Version of optimize for incomplete BayesNet, needs solution for missing variables
|
||||
VectorValues optimize(const VectorValues& solutionForMissing) const;
|
||||
/// Version of optimize for incomplete BayesNet, given missing variables
|
||||
VectorValues optimize(const VectorValues given) const;
|
||||
|
||||
/**
|
||||
* Sample using ancestral sampling
|
||||
* Example:
|
||||
* std::mt19937_64 rng(42);
|
||||
* auto sample = gbn.sample(&rng);
|
||||
*/
|
||||
VectorValues sample(std::mt19937_64* rng) const;
|
||||
|
||||
/**
|
||||
* Sample from an incomplete BayesNet, given missing variables
|
||||
* Example:
|
||||
* std::mt19937_64 rng(42);
|
||||
* VectorValues given = ...;
|
||||
* auto sample = gbn.sample(given, &rng);
|
||||
*/
|
||||
VectorValues sample(VectorValues given, std::mt19937_64* rng) const;
|
||||
|
||||
/// Sample using ancestral sampling, use default rng
|
||||
VectorValues sample() const;
|
||||
|
||||
/// Sample from an incomplete BayesNet, use default rng
|
||||
VectorValues sample(VectorValues given) const;
|
||||
|
||||
/**
|
||||
* Return ordering corresponding to a topological sort.
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
#include <gtsam/linear/linearExceptions.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/Sampler.h>
|
||||
|
||||
#include <boost/format.hpp>
|
||||
#ifdef __GNUC__
|
||||
|
|
@ -34,6 +35,9 @@
|
|||
#include <list>
|
||||
#include <string>
|
||||
|
||||
// In Wrappers we have no access to this so have a default ready
|
||||
static std::mt19937_64 kRandomNumberGenerator(42);
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -43,19 +47,47 @@ namespace gtsam {
|
|||
Key key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas) :
|
||||
BaseFactor(key, R, d, sigmas), BaseConditional(1) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianConditional::GaussianConditional(
|
||||
Key key, const Vector& d, const Matrix& R,
|
||||
Key name1, const Matrix& S, const SharedDiagonal& sigmas) :
|
||||
BaseFactor(key, R, name1, S, d, sigmas), BaseConditional(1) {}
|
||||
/* ************************************************************************ */
|
||||
GaussianConditional::GaussianConditional(Key key, const Vector& d,
|
||||
const Matrix& R, Key parent1,
|
||||
const Matrix& S,
|
||||
const SharedDiagonal& sigmas)
|
||||
: BaseFactor(key, R, parent1, S, d, sigmas), BaseConditional(1) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianConditional::GaussianConditional(
|
||||
Key key, const Vector& d, const Matrix& R,
|
||||
Key name1, const Matrix& S, Key name2, const Matrix& T, const SharedDiagonal& sigmas) :
|
||||
BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {}
|
||||
/* ************************************************************************ */
|
||||
GaussianConditional::GaussianConditional(Key key, const Vector& d,
|
||||
const Matrix& R, Key parent1,
|
||||
const Matrix& S, Key parent2,
|
||||
const Matrix& T,
|
||||
const SharedDiagonal& sigmas)
|
||||
: BaseFactor(key, R, parent1, S, parent2, T, d, sigmas),
|
||||
BaseConditional(1) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* ************************************************************************ */
|
||||
GaussianConditional GaussianConditional::FromMeanAndStddev(
|
||||
Key key, const Matrix& A, Key parent, const Vector& b, double sigma) {
|
||||
// |Rx + Sy - d| = |x-(Ay + b)|/sigma
|
||||
const Matrix R = Matrix::Identity(b.size(), b.size());
|
||||
const Matrix S = -A;
|
||||
const Vector d = b;
|
||||
return GaussianConditional(key, d, R, parent, S,
|
||||
noiseModel::Isotropic::Sigma(b.size(), sigma));
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
GaussianConditional GaussianConditional::FromMeanAndStddev(
|
||||
Key key, const Matrix& A1, Key parent1, const Matrix& A2, Key parent2,
|
||||
const Vector& b, double sigma) {
|
||||
// |Rx + Sy + Tz - d| = |x-(A1 y + A2 z + b)|/sigma
|
||||
const Matrix R = Matrix::Identity(b.size(), b.size());
|
||||
const Matrix S = -A1;
|
||||
const Matrix T = -A2;
|
||||
const Vector d = b;
|
||||
return GaussianConditional(key, d, R, parent1, S, parent2, T,
|
||||
noiseModel::Isotropic::Sigma(b.size(), sigma));
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
|
||||
cout << s << " Conditional density ";
|
||||
for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
|
||||
|
|
@ -192,7 +224,44 @@ namespace gtsam {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* ************************************************************************ */
|
||||
VectorValues GaussianConditional::sample(const VectorValues& parentsValues,
|
||||
std::mt19937_64* rng) const {
|
||||
if (nrFrontals() != 1) {
|
||||
throw std::invalid_argument(
|
||||
"GaussianConditional::sample can only be called on single variable "
|
||||
"conditionals");
|
||||
}
|
||||
if (!model_) {
|
||||
throw std::invalid_argument(
|
||||
"GaussianConditional::sample can only be called if a diagonal noise "
|
||||
"model was specified at construction.");
|
||||
}
|
||||
VectorValues solution = solve(parentsValues);
|
||||
Key key = firstFrontalKey();
|
||||
const Vector& sigmas = model_->sigmas();
|
||||
solution[key] += Sampler::sampleDiagonal(sigmas, rng);
|
||||
return solution;
|
||||
}
|
||||
|
||||
VectorValues GaussianConditional::sample(std::mt19937_64* rng) const {
|
||||
if (nrParents() != 0)
|
||||
throw std::invalid_argument(
|
||||
"sample() can only be invoked on no-parent prior");
|
||||
VectorValues values;
|
||||
return sample(values);
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
VectorValues GaussianConditional::sample() const {
|
||||
return sample(&kRandomNumberGenerator);
|
||||
}
|
||||
|
||||
VectorValues GaussianConditional::sample(const VectorValues& given) const {
|
||||
return sample(given, &kRandomNumberGenerator);
|
||||
}
|
||||
|
||||
/* ************************************************************************ */
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
|
||||
void GTSAM_DEPRECATED
|
||||
GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
|
||||
|
|
|
|||
|
|
@ -26,12 +26,15 @@
|
|||
#include <gtsam/inference/Conditional.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
#include <random> // for std::mt19937_64
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A conditional Gaussian functions as the node in a Bayes network
|
||||
* A GaussianConditional functions as the node in a Bayes network.
|
||||
* It has a set of parents y,z, etc. and implements a probability density on x.
|
||||
* The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$
|
||||
* @addtogroup linear
|
||||
*/
|
||||
class GTSAM_EXPORT GaussianConditional :
|
||||
public JacobianFactor,
|
||||
|
|
@ -43,6 +46,9 @@ namespace gtsam {
|
|||
typedef JacobianFactor BaseFactor; ///< Typedef to our factor base class
|
||||
typedef Conditional<BaseFactor, This> BaseConditional; ///< Typedef to our conditional base class
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/** default constructor needed for serialization */
|
||||
GaussianConditional() {}
|
||||
|
||||
|
|
@ -51,13 +57,14 @@ namespace gtsam {
|
|||
const SharedDiagonal& sigmas = SharedDiagonal());
|
||||
|
||||
/** constructor with only one parent |Rx+Sy-d| */
|
||||
GaussianConditional(Key key, const Vector& d, const Matrix& R,
|
||||
Key name1, const Matrix& S, const SharedDiagonal& sigmas = SharedDiagonal());
|
||||
GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
|
||||
const Matrix& S,
|
||||
const SharedDiagonal& sigmas = SharedDiagonal());
|
||||
|
||||
/** constructor with two parents |Rx+Sy+Tz-d| */
|
||||
GaussianConditional(Key key, const Vector& d, const Matrix& R,
|
||||
Key name1, const Matrix& S, Key name2, const Matrix& T,
|
||||
const SharedDiagonal& sigmas = SharedDiagonal());
|
||||
GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
|
||||
const Matrix& S, Key parent2, const Matrix& T,
|
||||
const SharedDiagonal& sigmas = SharedDiagonal());
|
||||
|
||||
/** Constructor with arbitrary number of frontals and parents.
|
||||
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
|
||||
|
|
@ -76,6 +83,17 @@ namespace gtsam {
|
|||
const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix,
|
||||
const SharedDiagonal& sigmas = SharedDiagonal());
|
||||
|
||||
/// Construct from mean A1 p1 + b and standard deviation.
|
||||
static GaussianConditional FromMeanAndStddev(Key key, const Matrix& A,
|
||||
Key parent, const Vector& b,
|
||||
double sigma);
|
||||
|
||||
/// Construct from mean A1 p1 + A2 p2 + b and standard deviation.
|
||||
static GaussianConditional FromMeanAndStddev(Key key, //
|
||||
const Matrix& A1, Key parent1,
|
||||
const Matrix& A2, Key parent2,
|
||||
const Vector& b, double sigma);
|
||||
|
||||
/** Combine several GaussianConditional into a single dense GC. The conditionals enumerated by
|
||||
* \c first and \c last must be in increasing order, meaning that the parents of any
|
||||
* conditional may not include a conditional coming before it.
|
||||
|
|
@ -86,6 +104,10 @@ namespace gtsam {
|
|||
template<typename ITERATOR>
|
||||
static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** print */
|
||||
void print(const std::string& = "GaussianConditional",
|
||||
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
|
||||
|
|
@ -93,6 +115,10 @@ namespace gtsam {
|
|||
/** equals function */
|
||||
bool equals(const GaussianFactor&cg, double tol = 1e-9) const override;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Return a view of the upper-triangular R block of the conditional */
|
||||
constABlock R() const { return Ab_.range(0, nrFrontals()); }
|
||||
|
||||
|
|
@ -125,10 +151,39 @@ namespace gtsam {
|
|||
/** Performs transpose backsubstition in place on values */
|
||||
void solveTransposeInPlace(VectorValues& gy) const;
|
||||
|
||||
/**
|
||||
* Sample from conditional, zero parent version
|
||||
* Example:
|
||||
* std::mt19937_64 rng(42);
|
||||
* auto sample = gbn.sample(&rng);
|
||||
*/
|
||||
VectorValues sample(std::mt19937_64* rng) const;
|
||||
|
||||
/**
|
||||
* Sample from conditional, given missing variables
|
||||
* Example:
|
||||
* std::mt19937_64 rng(42);
|
||||
* VectorValues given = ...;
|
||||
* auto sample = gbn.sample(given, &rng);
|
||||
*/
|
||||
VectorValues sample(const VectorValues& parentsValues,
|
||||
std::mt19937_64* rng) const;
|
||||
|
||||
/// Sample, use default rng
|
||||
VectorValues sample() const;
|
||||
|
||||
/// Sample with given values, use default rng
|
||||
VectorValues sample(const VectorValues& parentsValues) const;
|
||||
|
||||
/// @}
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
|
||||
/// @name Deprecated
|
||||
/// @{
|
||||
/** Scale the values in \c gy according to the sigmas for the frontal variables in this
|
||||
* conditional. */
|
||||
void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const;
|
||||
/// @}
|
||||
#endif
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -23,9 +23,12 @@ using namespace std;
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean, const double& sigma)
|
||||
{
|
||||
return GaussianDensity(key, mean / sigma, Matrix::Identity(mean.size(), mean.size()) / sigma);
|
||||
GaussianDensity GaussianDensity::FromMeanAndStddev(Key key,
|
||||
const Vector& mean,
|
||||
double sigma) {
|
||||
return GaussianDensity(key, mean,
|
||||
Matrix::Identity(mean.size(), mean.size()),
|
||||
noiseModel::Isotropic::Sigma(mean.size(), sigma));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -35,8 +38,8 @@ namespace gtsam {
|
|||
for(const_iterator it = beginFrontals(); it != endFrontals(); ++it)
|
||||
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
|
||||
cout << endl;
|
||||
gtsam::print(Matrix(R()), "R: ");
|
||||
gtsam::print(Vector(d()), "d: ");
|
||||
gtsam::print(mean(), "mean: ");
|
||||
gtsam::print(covariance(), "covariance: ");
|
||||
if(model_)
|
||||
model_->print("Noise model: ");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,11 +24,10 @@
|
|||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A Gaussian density.
|
||||
*
|
||||
* It is implemented as a GaussianConditional without parents.
|
||||
* A GaussianDensity is a GaussianConditional without parents.
|
||||
* The negative log-probability is given by \f$ |Rx - d|^2 \f$
|
||||
* with \f$ \Lambda = \Sigma^{-1} = R^T R \f$ and \f$ \mu = R^{-1} d \f$
|
||||
* @addtogroup linear
|
||||
*/
|
||||
class GTSAM_EXPORT GaussianDensity : public GaussianConditional {
|
||||
|
||||
|
|
@ -52,8 +51,9 @@ namespace gtsam {
|
|||
GaussianDensity(Key key, const Vector& d, const Matrix& R, const SharedDiagonal& noiseModel = SharedDiagonal()) :
|
||||
GaussianConditional(key, d, R, noiseModel) {}
|
||||
|
||||
/// Construct using a mean and variance
|
||||
static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean, const double& sigma);
|
||||
/// Construct using a mean and standard deviation
|
||||
static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean,
|
||||
double sigma);
|
||||
|
||||
/// print
|
||||
void print(const std::string& = "GaussianDensity",
|
||||
|
|
|
|||
|
|
@ -22,14 +22,18 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model,
|
||||
uint_fast64_t seed)
|
||||
: model_(model), generator_(seed) {}
|
||||
: model_(model), generator_(seed) {
|
||||
if (!model) {
|
||||
throw std::invalid_argument("Sampler::Sampler needs a non-null model.");
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed)
|
||||
: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
|
||||
Vector Sampler::sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng) {
|
||||
size_t d = sigmas.size();
|
||||
Vector result(d);
|
||||
for (size_t i = 0; i < d; i++) {
|
||||
|
|
@ -39,14 +43,18 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
|
|||
if (sigma == 0.0) {
|
||||
result(i) = 0.0;
|
||||
} else {
|
||||
typedef std::normal_distribution<double> Normal;
|
||||
Normal dist(0.0, sigma);
|
||||
result(i) = dist(generator_);
|
||||
std::normal_distribution<double> dist(0.0, sigma);
|
||||
result(i) = dist(*rng);
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
|
||||
return sampleDiagonal(sigmas, &generator_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Sampler::sample() const {
|
||||
assert(model_.get());
|
||||
|
|
|
|||
|
|
@ -63,15 +63,9 @@ class GTSAM_EXPORT Sampler {
|
|||
/// @name access functions
|
||||
/// @{
|
||||
|
||||
size_t dim() const {
|
||||
assert(model_.get());
|
||||
return model_->dim();
|
||||
}
|
||||
size_t dim() const { return model_->dim(); }
|
||||
|
||||
Vector sigmas() const {
|
||||
assert(model_.get());
|
||||
return model_->sigmas();
|
||||
}
|
||||
Vector sigmas() const { return model_->sigmas(); }
|
||||
|
||||
const noiseModel::Diagonal::shared_ptr& model() const { return model_; }
|
||||
|
||||
|
|
@ -82,6 +76,8 @@ class GTSAM_EXPORT Sampler {
|
|||
/// sample from distribution
|
||||
Vector sample() const;
|
||||
|
||||
/// sample with given random number generator
|
||||
static Vector sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng);
|
||||
/// @}
|
||||
|
||||
protected:
|
||||
|
|
|
|||
|
|
@ -468,15 +468,33 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
|
|||
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
|
||||
size_t name2, Matrix T);
|
||||
|
||||
// Standard Interface
|
||||
// Named constructors
|
||||
static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,
|
||||
const Matrix& A,
|
||||
gtsam::Key parent,
|
||||
const Vector& b,
|
||||
double sigma);
|
||||
|
||||
static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key,
|
||||
const Matrix& A1,
|
||||
gtsam::Key parent1,
|
||||
const Matrix& A2,
|
||||
gtsam::Key parent2,
|
||||
const Vector& b,
|
||||
double sigma);
|
||||
// Testable
|
||||
void print(string s = "GaussianConditional",
|
||||
const gtsam::KeyFormatter& keyFormatter =
|
||||
gtsam::DefaultKeyFormatter) const;
|
||||
bool equals(const gtsam::GaussianConditional& cg, double tol) const;
|
||||
|
||||
// Standard Interface
|
||||
gtsam::Key firstFrontalKey() const;
|
||||
gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
|
||||
gtsam::VectorValues sample(const gtsam::VectorValues& parents) const;
|
||||
gtsam::VectorValues sample() const;
|
||||
|
||||
// Advanced Interface
|
||||
gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
|
||||
gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents,
|
||||
const gtsam::VectorValues& rhs) const;
|
||||
void solveTransposeInPlace(gtsam::VectorValues& gy) const;
|
||||
|
|
@ -490,14 +508,21 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
|
|||
|
||||
#include <gtsam/linear/GaussianDensity.h>
|
||||
virtual class GaussianDensity : gtsam::GaussianConditional {
|
||||
//Constructors
|
||||
GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
|
||||
// Constructors
|
||||
GaussianDensity(gtsam::Key key, Vector d, Matrix R,
|
||||
const gtsam::noiseModel::Diagonal* sigmas);
|
||||
|
||||
//Standard Interface
|
||||
static gtsam::GaussianDensity FromMeanAndStddev(gtsam::Key key,
|
||||
const Vector& mean,
|
||||
double sigma);
|
||||
|
||||
// Testable
|
||||
void print(string s = "GaussianDensity",
|
||||
const gtsam::KeyFormatter& keyFormatter =
|
||||
gtsam::DefaultKeyFormatter) const;
|
||||
bool equals(const gtsam::GaussianDensity &cg, double tol) const;
|
||||
bool equals(const gtsam::GaussianDensity& cg, double tol) const;
|
||||
|
||||
// Standard Interface
|
||||
Vector mean() const;
|
||||
Matrix covariance() const;
|
||||
};
|
||||
|
|
@ -514,6 +539,21 @@ virtual class GaussianBayesNet {
|
|||
bool equals(const gtsam::GaussianBayesNet& other, double tol) const;
|
||||
size_t size() const;
|
||||
|
||||
// Standard interface
|
||||
void push_back(gtsam::GaussianConditional* conditional);
|
||||
void push_back(const gtsam::GaussianBayesNet& bayesNet);
|
||||
gtsam::GaussianConditional* front() const;
|
||||
gtsam::GaussianConditional* back() const;
|
||||
|
||||
gtsam::VectorValues optimize() const;
|
||||
gtsam::VectorValues optimize(gtsam::VectorValues given) const;
|
||||
gtsam::VectorValues optimizeGradientSearch() const;
|
||||
|
||||
gtsam::VectorValues sample(gtsam::VectorValues given) const;
|
||||
gtsam::VectorValues sample() const;
|
||||
gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const;
|
||||
gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const;
|
||||
|
||||
// FactorGraph derived interface
|
||||
gtsam::GaussianConditional* at(size_t idx) const;
|
||||
gtsam::KeySet keys() const;
|
||||
|
|
@ -522,22 +562,12 @@ virtual class GaussianBayesNet {
|
|||
|
||||
void saveGraph(const string& s) const;
|
||||
|
||||
gtsam::GaussianConditional* front() const;
|
||||
gtsam::GaussianConditional* back() const;
|
||||
void push_back(gtsam::GaussianConditional* conditional);
|
||||
void push_back(const gtsam::GaussianBayesNet& bayesNet);
|
||||
|
||||
gtsam::VectorValues optimize() const;
|
||||
gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
|
||||
std::pair<Matrix, Vector> matrix() const;
|
||||
gtsam::VectorValues optimizeGradientSearch() const;
|
||||
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
|
||||
gtsam::VectorValues gradientAtZero() const;
|
||||
double error(const gtsam::VectorValues& x) const;
|
||||
double determinant() const;
|
||||
double logDeterminant() const;
|
||||
gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const;
|
||||
gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const;
|
||||
|
||||
string dot(
|
||||
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
|
||||
|
|
|
|||
|
|
@ -16,10 +16,12 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianDensity.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
|
|
@ -35,6 +37,7 @@ using namespace boost::assign;
|
|||
using namespace std::placeholders;
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using symbol_shorthand::X;
|
||||
|
||||
static const Key _x_ = 11, _y_ = 22, _z_ = 33;
|
||||
|
||||
|
|
@ -138,6 +141,30 @@ TEST( GaussianBayesNet, optimize3 )
|
|||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianBayesNet, sample) {
|
||||
GaussianBayesNet gbn;
|
||||
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
|
||||
const Vector2 mean(20, 40), b(10, 10);
|
||||
const double sigma = 0.01;
|
||||
|
||||
gbn.add(GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma));
|
||||
gbn.add(GaussianDensity::FromMeanAndStddev(X(1), mean, sigma));
|
||||
|
||||
auto actual = gbn.sample();
|
||||
EXPECT_LONGS_EQUAL(2, actual.size());
|
||||
EXPECT(assert_equal(mean, actual[X(1)], 50 * sigma));
|
||||
EXPECT(assert_equal(A1 * mean + b, actual[X(0)], 50 * sigma));
|
||||
|
||||
// Use a specific random generator
|
||||
std::mt19937_64 rng(4242);
|
||||
auto actual3 = gbn.sample(&rng);
|
||||
EXPECT_LONGS_EQUAL(2, actual.size());
|
||||
// regression is not repeatable across platforms/versions :-(
|
||||
// EXPECT(assert_equal(Vector2(20.0129382, 40.0039798), actual[X(1)], 1e-5));
|
||||
// EXPECT(assert_equal(Vector2(110.032083, 230.039811), actual[X(0)], 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianBayesNet, ordering)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -20,9 +20,10 @@
|
|||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/VerticalBlockMatrix.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/linear/GaussianDensity.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
|
||||
#include <boost/assign/std/list.hpp>
|
||||
|
|
@ -38,6 +39,7 @@
|
|||
using namespace gtsam;
|
||||
using namespace std;
|
||||
using namespace boost::assign;
|
||||
using symbol_shorthand::X;
|
||||
|
||||
static const double tol = 1e-5;
|
||||
|
||||
|
|
@ -316,5 +318,60 @@ TEST( GaussianConditional, isGaussianFactor ) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
// Test FromMeanAndStddev named constructors
|
||||
TEST(GaussianConditional, FromMeanAndStddev) {
|
||||
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
|
||||
Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.).finished();
|
||||
const Vector2 b(20, 40), x0(1, 2), x1(3, 4), x2(5, 6);
|
||||
const double sigma = 3;
|
||||
|
||||
VectorValues values = map_list_of(X(0), x0)(X(1), x1)(X(2), x2);
|
||||
|
||||
auto conditional1 =
|
||||
GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
|
||||
Vector2 e1 = (x0 - (A1 * x1 + b)) / sigma;
|
||||
double expected1 = 0.5 * e1.dot(e1);
|
||||
EXPECT_DOUBLES_EQUAL(expected1, conditional1.error(values), 1e-9);
|
||||
|
||||
auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), A2,
|
||||
X(2), b, sigma);
|
||||
Vector2 e2 = (x0 - (A1 * x1 + A2 * x2 + b)) / sigma;
|
||||
double expected2 = 0.5 * e2.dot(e2);
|
||||
EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test sampling
|
||||
TEST(GaussianConditional, sample) {
|
||||
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
|
||||
const Vector2 b(20, 40), x1(3, 4);
|
||||
const double sigma = 0.01;
|
||||
|
||||
auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma);
|
||||
auto actual1 = density.sample();
|
||||
EXPECT_LONGS_EQUAL(1, actual1.size());
|
||||
EXPECT(assert_equal(b, actual1[X(0)], 50 * sigma));
|
||||
|
||||
VectorValues given;
|
||||
given.insert(X(1), x1);
|
||||
|
||||
auto conditional =
|
||||
GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
|
||||
auto actual2 = conditional.sample(given);
|
||||
EXPECT_LONGS_EQUAL(1, actual2.size());
|
||||
EXPECT(assert_equal(A1 * x1 + b, actual2[X(0)], 50 * sigma));
|
||||
|
||||
// Use a specific random generator
|
||||
std::mt19937_64 rng(4242);
|
||||
auto actual3 = conditional.sample(given, &rng);
|
||||
EXPECT_LONGS_EQUAL(1, actual2.size());
|
||||
// regression is not repeatable across platforms/versions :-(
|
||||
// EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -17,10 +17,13 @@
|
|||
**/
|
||||
|
||||
#include <gtsam/linear/GaussianDensity.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
using symbol_shorthand::X;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(GaussianDensity, constructor)
|
||||
|
|
@ -37,6 +40,22 @@ TEST(GaussianDensity, constructor)
|
|||
EXPECT(assert_equal(s, copied.get_model()->sigmas()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test FromMeanAndStddev named constructor
|
||||
TEST(GaussianDensity, FromMeanAndStddev) {
|
||||
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
|
||||
const Vector2 b(20, 40), x0(1, 2);
|
||||
const double sigma = 3;
|
||||
|
||||
VectorValues values;
|
||||
values.insert(X(0), x0);
|
||||
|
||||
auto density = GaussianDensity::FromMeanAndStddev(X(0), b, sigma);
|
||||
Vector2 e = (x0 - b) / sigma;
|
||||
double expected = 0.5 * e.dot(e);
|
||||
EXPECT_DOUBLES_EQUAL(expected, density.error(values), 1e-9);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -112,54 +112,50 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
|
|||
C4 x7 : x6
|
||||
|
||||
************************************************************************* */
|
||||
TEST( GaussianBayesTree, balanced_smoother_marginals )
|
||||
{
|
||||
TEST(GaussianBayesTree, balanced_smoother_marginals) {
|
||||
// Create smoother with 7 nodes
|
||||
GaussianFactorGraph smoother = createSmoother(7);
|
||||
|
||||
// Create the Bayes tree
|
||||
Ordering ordering;
|
||||
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
|
||||
ordering += X(1), X(3), X(5), X(7), X(2), X(6), X(4);
|
||||
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
|
||||
|
||||
VectorValues actualSolution = bayesTree.optimize();
|
||||
VectorValues expectedSolution = VectorValues::Zero(actualSolution);
|
||||
EXPECT(assert_equal(expectedSolution,actualSolution,tol));
|
||||
EXPECT(assert_equal(expectedSolution, actualSolution, tol));
|
||||
|
||||
LONGS_EQUAL(4, (long)bayesTree.size());
|
||||
LONGS_EQUAL(4, bayesTree.size());
|
||||
|
||||
double tol=1e-5;
|
||||
double tol = 1e-5;
|
||||
|
||||
// Check marginal on x1
|
||||
JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1);
|
||||
JacobianFactor actual1 = *bayesTree.marginalFactor(X(1));
|
||||
Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1);
|
||||
Matrix actualCovarianceX1;
|
||||
GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky);
|
||||
actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse();
|
||||
EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
|
||||
EXPECT(assert_equal(expected1,actual1,tol));
|
||||
Matrix expectedCovX1 = I_2x2 * (sigmax1 * sigmax1);
|
||||
auto m = bayesTree.marginalFactor(X(1), EliminateCholesky);
|
||||
Matrix actualCovarianceX1 = m->information().inverse();
|
||||
EXPECT(assert_equal(expectedCovX1, actualCovarianceX1, tol));
|
||||
|
||||
// Check marginal on x2
|
||||
double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
|
||||
JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2);
|
||||
double sigmax2 = 0.68712938; // FIXME: this should be corrected analytically
|
||||
JacobianFactor actual2 = *bayesTree.marginalFactor(X(2));
|
||||
EXPECT(assert_equal(expected2,actual2,tol));
|
||||
Matrix expectedCovX2 = I_2x2 * (sigmax2 * sigmax2);
|
||||
EXPECT(assert_equal(expectedCovX2, actual2.information().inverse(), tol));
|
||||
|
||||
// Check marginal on x3
|
||||
JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3);
|
||||
JacobianFactor actual3 = *bayesTree.marginalFactor(X(3));
|
||||
EXPECT(assert_equal(expected3,actual3,tol));
|
||||
Matrix expectedCovX3 = I_2x2 * (sigmax3 * sigmax3);
|
||||
EXPECT(assert_equal(expectedCovX3, actual3.information().inverse(), tol));
|
||||
|
||||
// Check marginal on x4
|
||||
JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4);
|
||||
JacobianFactor actual4 = *bayesTree.marginalFactor(X(4));
|
||||
EXPECT(assert_equal(expected4,actual4,tol));
|
||||
Matrix expectedCovX4 = I_2x2 * (sigmax4 * sigmax4);
|
||||
EXPECT(assert_equal(expectedCovX4, actual4.information().inverse(), tol));
|
||||
|
||||
// Check marginal on x7 (should be equal to x1)
|
||||
JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7);
|
||||
JacobianFactor actual7 = *bayesTree.marginalFactor(X(7));
|
||||
EXPECT(assert_equal(expected7,actual7,tol));
|
||||
Matrix expectedCovX7 = I_2x2 * (sigmax7 * sigmax7);
|
||||
EXPECT(assert_equal(expectedCovX7, actual7.information().inverse(), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue