diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index d9cde9b91..2bae129be 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -23,9 +23,11 @@ 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 / sigma, + Matrix::Identity(mean.size(), mean.size()) / sigma); } /* ************************************************************************* */ @@ -35,8 +37,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: "); } diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 71af704ab..f078d5db6 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -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", diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 674287b87..52beb11f6 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -490,14 +490,19 @@ virtual class GaussianConditional : gtsam::JacobianFactor { #include 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); + + // Standard Interface 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; Vector mean() const; Matrix covariance() const; };