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

Merge branch 'develop' into release/4.2a5

release/4.3a0
Frank Dellaert 2022-02-12 14:19:48 -05:00
parent 4a1ec24de5 3e65779421
commit 46f3a48a5b
24 changed files with 638 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/base/FastSet.h
View File

@ -18,6 +18,9 @@
#pragma once
#if BOOST_VERSION >= 107400
#include <boost/serialization/library_version_type.hpp>
#endif
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/set.hpp>
#include <gtsam/base/FastDefaultAllocator.h>

1
gtsam/base/serialization.h
View File

@ -25,6 +25,7 @@
#include <string>
// includes for standard serialization types
#include <boost/serialization/version.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/vector.hpp>

4
gtsam/discrete/DiscreteConditional.cpp
View File

@ -225,13 +225,13 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
/* ****************************************************************************/
DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood(
size_t parent_value) const {
size_t frontal) const {
if (nrFrontals() != 1)
throw std::invalid_argument(
"Single value likelihood can only be invoked on single-variable "
"conditional");
DiscreteValues values;
values.emplace(keys_[0], parent_value);
values.emplace(keys_[0], frontal);
return likelihood(values);
}

2
gtsam/discrete/DiscreteConditional.h
View File

@ -177,7 +177,7 @@ class GTSAM_EXPORT DiscreteConditional
const DiscreteValues& frontalValues) const;
/** Single variable version of likelihood. */
DecisionTreeFactor::shared_ptr likelihood(size_t parent_value) const;
DecisionTreeFactor::shared_ptr likelihood(size_t frontal) const;
/**
* sample

1
gtsam/discrete/tests/testDiscreteBayesNet.cpp
View File

@ -150,7 +150,6 @@ TEST(DiscreteBayesNet, Dot) {
fragment.add((Either | Tuberculosis, LungCancer) = "F T T T");
string actual = fragment.dot();
cout << actual << endl;
EXPECT(actual ==
"digraph {\n"
" size=\"5,5\";\n"

55
gtsam/linear/GaussianBayesNet.cpp
View File

@ -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);

30
gtsam/linear/GaussianBayesNet.h
View File

@ -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.

137
gtsam/linear/GaussianConditional.cpp
View File

@ -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,88 @@ namespace gtsam {
}
}
/* ************************************************************************* */
/* ************************************************************************ */
JacobianFactor::shared_ptr GaussianConditional::likelihood(
const VectorValues& frontalValues) const {
// Error is |Rx - (d - Sy - Tz - ...)|^2
// so when we instantiate x (which has to be completely known) we beget:
// |Sy + Tz + ... - (d - Rx)|^2
// The noise model just transfers over!
// Get frontalValues as vector
const Vector x =
frontalValues.vector(KeyVector(beginFrontals(), endFrontals()));
// Copy the augmented Jacobian matrix:
auto newAb = Ab_;
// Restrict view to parent blocks
newAb.firstBlock() += nrFrontals_;
// Update right-hand-side (last column)
auto last = newAb.matrix().cols() - 1;
const auto RR = R().triangularView<Eigen::Upper>();
newAb.matrix().col(last) -= RR * x;
// The keys now do not include the frontal keys:
KeyVector newKeys;
newKeys.reserve(nrParents());
for (auto&& key : parents()) newKeys.push_back(key);
// Hopefully second newAb copy below is optimized out...
return boost::make_shared<JacobianFactor>(newKeys, newAb, model_);
}
/* **************************************************************************/
JacobianFactor::shared_ptr GaussianConditional::likelihood(
const Vector& frontal) const {
if (nrFrontals() != 1)
throw std::invalid_argument(
"GaussianConditional Single value likelihood can only be invoked on "
"single-variable conditional");
VectorValues values;
values.insert(keys_[0], frontal);
return likelihood(values);
}
/* ************************************************************************ */
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 {

74
gtsam/linear/GaussianConditional.h
View File

@ -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,46 @@ namespace gtsam {
/** Performs transpose backsubstition in place on values */
void solveTransposeInPlace(VectorValues& gy) const;
/** Convert to a likelihood factor by providing value before bar. */
JacobianFactor::shared_ptr likelihood(
const VectorValues& frontalValues) const;
/** Single variable version of likelihood. */
JacobianFactor::shared_ptr likelihood(const Vector& frontal) 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:

13
gtsam/linear/GaussianDensity.cpp
View File

@ -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: ");
}

10
gtsam/linear/GaussianDensity.h
View File

@ -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",

18
gtsam/linear/Sampler.cpp
View File

@ -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());

12
gtsam/linear/Sampler.h
View File

@ -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:

93
gtsam/linear/VectorValues.cpp
View File

@ -33,7 +33,7 @@ namespace gtsam {
using boost::adaptors::map_values;
using boost::accumulate;
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues::VectorValues(const VectorValues& first, const VectorValues& second)
{
// Merge using predicate for comparing first of pair
@ -44,7 +44,7 @@ namespace gtsam {
throw invalid_argument("Requested to merge two VectorValues that have one or more variables in common.");
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues::VectorValues(const Vector& x, const Dims& dims) {
using Pair = pair<const Key, size_t>;
size_t j = 0;
@ -61,7 +61,7 @@ namespace gtsam {
}
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues::VectorValues(const Vector& x, const Scatter& scatter) {
size_t j = 0;
for (const SlotEntry& v : scatter) {
@ -74,7 +74,7 @@ namespace gtsam {
}
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::Zero(const VectorValues& other)
{
VectorValues result;
@ -87,7 +87,7 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues::iterator VectorValues::insert(const std::pair<Key, Vector>& key_value) {
std::pair<iterator, bool> result = values_.insert(key_value);
if(!result.second)
@ -97,7 +97,7 @@ namespace gtsam {
return result.first;
}
/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::update(const VectorValues& values)
{
iterator hint = begin();
@ -115,7 +115,7 @@ namespace gtsam {
}
}
/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::insert(const VectorValues& values)
{
size_t originalSize = size();
@ -124,14 +124,14 @@ namespace gtsam {
throw invalid_argument("Requested to insert a VectorValues into another VectorValues that already contains one or more of its keys.");
}
/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::setZero()
{
for(Vector& v: values_ | map_values)
v.setZero();
}
/* ************************************************************************* */
/* ************************************************************************ */
GTSAM_EXPORT ostream& operator<<(ostream& os, const VectorValues& v) {
// Change print depending on whether we are using TBB
#ifdef GTSAM_USE_TBB
@ -150,7 +150,7 @@ namespace gtsam {
return os;
}
/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::print(const string& str,
const KeyFormatter& formatter) const {
cout << str << ": " << size() << " elements\n";
@ -158,7 +158,7 @@ namespace gtsam {
cout.flush();
}
/* ************************************************************************* */
/* ************************************************************************ */
bool VectorValues::equals(const VectorValues& x, double tol) const {
if(this->size() != x.size())
return false;
@ -170,7 +170,7 @@ namespace gtsam {
return true;
}
/* ************************************************************************* */
/* ************************************************************************ */
Vector VectorValues::vector() const {
// Count dimensions
DenseIndex totalDim = 0;
@ -187,7 +187,7 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
Vector VectorValues::vector(const Dims& keys) const
{
// Count dimensions
@ -203,12 +203,12 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
void VectorValues::swap(VectorValues& other) {
this->values_.swap(other.values_);
}
/* ************************************************************************* */
/* ************************************************************************ */
namespace internal
{
bool structureCompareOp(const boost::tuple<VectorValues::value_type,
@ -219,14 +219,14 @@ namespace gtsam {
}
}
/* ************************************************************************* */
/* ************************************************************************ */
bool VectorValues::hasSameStructure(const VectorValues other) const
{
return accumulate(combine(*this, other)
| transformed(internal::structureCompareOp), true, logical_and<bool>());
}
/* ************************************************************************* */
/* ************************************************************************ */
double VectorValues::dot(const VectorValues& v) const
{
if(this->size() != v.size())
@ -244,12 +244,12 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
double VectorValues::norm() const {
return std::sqrt(this->squaredNorm());
}
/* ************************************************************************* */
/* ************************************************************************ */
double VectorValues::squaredNorm() const {
double sumSquares = 0.0;
using boost::adaptors::map_values;
@ -258,7 +258,7 @@ namespace gtsam {
return sumSquares;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::operator+(const VectorValues& c) const
{
if(this->size() != c.size())
@ -278,13 +278,13 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::add(const VectorValues& c) const
{
return *this + c;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::operator+=(const VectorValues& c)
{
if(this->size() != c.size())
@ -301,13 +301,13 @@ namespace gtsam {
return *this;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::addInPlace(const VectorValues& c)
{
return *this += c;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::addInPlace_(const VectorValues& c)
{
for(const_iterator j2 = c.begin(); j2 != c.end(); ++j2) {
@ -320,7 +320,7 @@ namespace gtsam {
return *this;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::operator-(const VectorValues& c) const
{
if(this->size() != c.size())
@ -340,13 +340,13 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::subtract(const VectorValues& c) const
{
return *this - c;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues operator*(const double a, const VectorValues &v)
{
VectorValues result;
@ -359,13 +359,13 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues VectorValues::scale(const double a) const
{
return a * *this;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::operator*=(double alpha)
{
for(Vector& v: *this | map_values)
@ -373,12 +373,43 @@ namespace gtsam {
return *this;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues& VectorValues::scaleInPlace(double alpha)
{
return *this *= alpha;
}
/* ************************************************************************* */
/* ************************************************************************ */
string VectorValues::html(const KeyFormatter& keyFormatter) const {
stringstream ss;
// Print out preamble.
ss << "<div>\n<table class='VectorValues'>\n <thead>\n";
// Print out header row.
ss << " <tr><th>Variable</th><th>value</th></tr>\n";
// Finish header and start body.
ss << " </thead>\n <tbody>\n";
// Print out all rows.
#ifdef GTSAM_USE_TBB
// TBB uses un-ordered map, so inefficiently order them:
std::map<Key, Vector> ordered;
for (const auto& kv : *this) ordered.emplace(kv);
for (const auto& kv : ordered) {
#else
for (const auto& kv : *this) {
#endif
ss << " <tr>";
ss << "<th>" << keyFormatter(kv.first) << "</th><td>"
<< kv.second.transpose() << "</td>";
ss << "</tr>\n";
}
ss << " </tbody>\n</table>\n</div>";
return ss.str();
}
/* ************************************************************************ */
} // \namespace gtsam

15
gtsam/linear/VectorValues.h
View File

@ -34,7 +34,7 @@
namespace gtsam {
/**
* This class represents a collection of vector-valued variables associated
* VectorValues represents a collection of vector-valued variables associated
* each with a unique integer index. It is typically used to store the variables
* of a GaussianFactorGraph. Optimizing a GaussianFactorGraph or GaussianBayesNet
* returns this class.
@ -69,7 +69,7 @@ namespace gtsam {
* which is a view on the underlying data structure.
*
* This class is additionally used in gradient descent and dog leg to store the gradient.
* \nosubgrouping
* @addtogroup linear
*/
class GTSAM_EXPORT VectorValues {
protected:
@ -344,11 +344,16 @@ namespace gtsam {
/// @}
/// @}
/// @name Matlab syntactic sugar for linear algebra operations
/// @name Wrapper support
/// @{
//inline VectorValues scale(const double a, const VectorValues& c) const { return a * (*this); }
/**
* @brief Output as a html table.
*
* @param keyFormatter function that formats keys.
*/
std::string html(
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}

66
gtsam/linear/linear.i
View File

@ -255,6 +255,7 @@ class VectorValues {
// enabling serialization functionality
void serialize() const;
string html() const;
};
#include <gtsam/linear/GaussianFactor.h>
@ -468,15 +469,36 @@ 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::JacobianFactor* likelihood(
const gtsam::VectorValues& frontalValues) const;
gtsam::JacobianFactor* likelihood(Vector frontal) 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 +512,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 +543,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 +566,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,

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

@ -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)
{

88
gtsam/linear/tests/testGaussianConditional.cpp
View File

@ -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,87 @@ 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 likelihood method (conversion to JacobianFactor)
TEST(GaussianConditional, likelihood) {
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
const Vector2 b(20, 40), x0(1, 2);
const double sigma = 0.01;
// |x0 - A1 x1 - b|^2
auto conditional =
GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
VectorValues frontalValues;
frontalValues.insert(X(0), x0);
auto actual1 = conditional.likelihood(frontalValues);
CHECK(actual1);
// |(-A1) x1 - (b - x0)|^2
JacobianFactor expected(X(1), -A1, b - x0,
noiseModel::Isotropic::Sigma(2, sigma));
EXPECT(assert_equal(expected, *actual1, tol));
// Check single vector version
auto actual2 = conditional.likelihood(x0);
CHECK(actual2);
EXPECT(assert_equal(expected, *actual2, tol));
}
/* ************************************************************************* */
// 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);
}
/* ************************************************************************* */

19
gtsam/linear/tests/testGaussianDensity.cpp
View File

@ -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);}
/* ************************************************************************* */

29
gtsam/linear/tests/testVectorValues.cpp
View File

@ -17,7 +17,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/LabeledSymbol.h>
#include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h>
@ -248,6 +248,33 @@ TEST(VectorValues, print)
EXPECT(expected == actual.str());
}
/* ************************************************************************* */
// Check html representation.
TEST(VectorValues, html) {
VectorValues vv;
using symbol_shorthand::X;
vv.insert(X(1), Vector2(2, 3.1));
vv.insert(X(2), Vector2(4, 5.2));
vv.insert(X(5), Vector2(6, 7.3));
vv.insert(X(7), Vector2(8, 9.4));
string expected =
"<div>\n"
"<table class='VectorValues'>\n"
" <thead>\n"
" <tr><th>Variable</th><th>value</th></tr>\n"
" </thead>\n"
" <tbody>\n"
" <tr><th>x1</th><td> 2 3.1</td></tr>\n"
" <tr><th>x2</th><td> 4 5.2</td></tr>\n"
" <tr><th>x5</th><td> 6 7.3</td></tr>\n"
" <tr><th>x7</th><td> 8 9.4</td></tr>\n"
" </tbody>\n"
"</table>\n"
"</div>";
string actual = vv.html();
EXPECT(actual == expected);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

4
gtsam/slam/FrobeniusFactor.h
View File

@ -54,6 +54,8 @@ class GTSAM_EXPORT FrobeniusPrior : public NoiseModelFactor1<Rot> {
Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Constructor
FrobeniusPrior(Key j, const MatrixNN& M,
const SharedNoiseModel& model = nullptr)
@ -106,6 +108,8 @@ class GTSAM_EXPORT FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// @name Constructor
/// @{

2
python/gtsam/preamble/inference.h
View File

@ -10,3 +10,5 @@
* Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++.
*/
#include <pybind11/stl.h>

35
python/gtsam/tests/test_DiscreteBayesNet.py
View File

@ -12,7 +12,9 @@ Author: Frank Dellaert
# pylint: disable=no-name-in-module, invalid-name
import unittest
import textwrap
import gtsam
from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph,
DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering)
from gtsam.utils.test_case import GtsamTestCase
@ -126,6 +128,39 @@ class TestDiscreteBayesNet(GtsamTestCase):
actual = fragment.sample(given)
self.assertEqual(len(actual), 5)
def test_dot(self):
"""Check that dot works with position hints."""
fragment = DiscreteBayesNet()
fragment.add(Either, [Tuberculosis, LungCancer], "F T T T")
MyAsia = gtsam.symbol('a', 0), 2 # use a symbol!
fragment.add(Tuberculosis, [MyAsia], "99/1 95/5")
fragment.add(LungCancer, [Smoking], "99/1 90/10")
# Make sure we can *update* position hints
writer = gtsam.DotWriter()
ph: dict = writer.positionHints
ph.update({'a': 2}) # hint at symbol position
writer.positionHints = ph
# Check the output of dot
actual = fragment.dot(writer=writer)
expected_result = """\
digraph {
size="5,5";
var3[label="3"];
var4[label="4"];
var5[label="5"];
var6[label="6"];
vara0[label="a0", pos="0,2!"];
var4->var6
vara0->var3
var3->var5
var6->var5
}"""
self.assertEqual(actual, textwrap.dedent(expected_result))
if __name__ == "__main__":
unittest.main()

40
tests/testGaussianBayesTreeB.cpp
View File

@ -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));
}
/* ************************************************************************* */