50 lines
1.5 KiB
C++
50 lines
1.5 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>
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
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(Matrix(get_R()), "R: ");
|
|
gtsam::print(Vector(get_d()), "d: ");
|
|
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();
|
|
}
|
|
|
|
} // gtsam
|