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

Working with sigma in matrices

release/4.3a0
Frank Dellaert 2022-02-06 12:53:27 -05:00
parent 6f74fe49c9
commit 476a2539f4
4 changed files with 99 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/linear/GaussianConditional.cpp
View File

@ -43,19 +43,45 @@ 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()) / sigma;
const Matrix S = -A / sigma;
const Vector d = b / sigma;
return GaussianConditional(key, d, R, parent, S);
}
/* ************************************************************************ */
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()) / sigma;
const Matrix S = -A1 / sigma;
const Matrix T = -A2 / sigma;
const Vector d = b / sigma;
return GaussianConditional(key, d, R, parent1, S, parent2, T);
}
/* ************************************************************************ */
void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
cout << s << " Conditional density ";
for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {

25
gtsam/linear/GaussianConditional.h
View File

@ -29,9 +29,10 @@
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,
@ -51,13 +52,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 +78,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.

14
gtsam/linear/linear.i
View File

@ -468,6 +468,20 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
size_t name2, Matrix T);
// 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);
// Standard Interface
void print(string s = "GaussianConditional",
const gtsam::KeyFormatter& keyFormatter =

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

@ -20,7 +20,7 @@
#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/GaussianBayesNet.h>
@ -38,6 +38,7 @@
using namespace gtsam;
using namespace std;
using namespace boost::assign;
using symbol_shorthand::X;
static const double tol = 1e-5;
@ -316,5 +317,31 @@ 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);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */