58 lines
1.9 KiB
C++
58 lines
1.9 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file GaussianDensity.cpp
|
|
* @brief A Gaussian Density
|
|
* @author Frank Dellaert
|
|
* @date Jan 21, 2012
|
|
*/
|
|
|
|
#include <gtsam/linear/GaussianDensity.h>
|
|
#include <boost/format.hpp>
|
|
#include <string>
|
|
|
|
using std::cout;
|
|
using std::endl;
|
|
using std::string;
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
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));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void GaussianDensity::print(const string& s,
|
|
const KeyFormatter& formatter) const {
|
|
cout << s << ": density on ";
|
|
for (const_iterator it = beginFrontals(); it != endFrontals(); ++it)
|
|
cout << (boost::format("[%1%]") % (formatter(*it))).str() << " ";
|
|
cout << endl;
|
|
gtsam::print(mean(), "mean: ");
|
|
gtsam::print(covariance(), "covariance: ");
|
|
if (model_) model_->print("Noise model: ");
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector GaussianDensity::mean() const {
|
|
VectorValues soln = this->solve(VectorValues());
|
|
return soln[firstFrontalKey()];
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Matrix GaussianDensity::covariance() const { return information().inverse(); }
|
|
|
|
} // namespace gtsam
|