diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index 8e37a677d..72e3bb325 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -22,6 +22,19 @@ using namespace std; namespace gtsam { + /* ************************************************************************* */ + void GaussianDensity::print(const string &s) const + { + cout << s << ": density on "; + for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) + cout << (boost::format("[%1%]")%(*it)).str() << " "; + cout << endl; + gtsam::print(Matrix(get_R()),"R"); + gtsam::print(Vector(get_d()),"d"); + gtsam::print(sigmas_,"sigmas"); + cout << "Permutation: " << permutation_.indices().transpose() << endl; + } + /* ************************************************************************* */ Vector GaussianDensity::mean() const { // Solve for mean diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 9ef0007ac..2c13e847c 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -34,16 +34,14 @@ namespace gtsam { public: - protected: - - public: + typedef boost::shared_ptr shared_ptr; /// default constructor needed for serialization GaussianDensity() : GaussianConditional() { } - // Copy constructor from GaussianConditional + /// Copy constructor from GaussianConditional GaussianDensity(const GaussianConditional& conditional) : GaussianConditional(conditional) { assert(conditional.nrParents() == 0); @@ -55,7 +53,10 @@ namespace gtsam { GaussianConditional(key, d, R, sigmas) { } - // Mean \f$ \mu = R^{-1} d \f$ + /// print + void print(const std::string& = "GaussianDensity") const; + + /// Mean \f$ \mu = R^{-1} d \f$ Vector mean() const; /// Information matrix \f$ \Lambda = R^T R \f$