testGaussianDensity.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#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)
{
  Matrix R = (Matrix(2,2) <<
      -12.1244,  -5.1962,
            0.,   4.6904).finished();

  Vector d = Vector2(1.0, 2.0), s = Vector2(3.0, 4.0);
  GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s));

  GaussianDensity copied(conditional);
  EXPECT(assert_equal(d, copied.d()));
  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 expected1 = 0.5 * e.dot(e);
  EXPECT_DOUBLES_EQUAL(expected1, density.error(values), 1e-9);

  double expected2 = density.logNormalizationConstant()- 0.5 * e.dot(e);
  EXPECT_DOUBLES_EQUAL(expected2, density.logProbability(values), 1e-9);
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */