diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp index caa9866b0..b7eae2931 100644 --- a/cpp/NoiseModel.cpp +++ b/cpp/NoiseModel.cpp @@ -85,26 +85,27 @@ namespace gtsam { for (j = 0; j < n; j++) variances(j) = covariance(j,j); return Diagonal::Variances(variances,true); } - full: return shared_ptr(new Gaussian(inverse_square_root(covariance))); + full: return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); } void Gaussian::print(const string& name) const { - gtsam::print(sqrt_information_, "Gaussian"); + gtsam::print(thisR(), "Gaussian"); } bool Gaussian::equals(const Base& expected, double tol) const { const Gaussian* p = dynamic_cast (&expected); if (p == NULL) return false; if (typeid(*this) != typeid(*p)) return false; - return equal_with_abs_tol(sqrt_information_, p->sqrt_information_, sqrt(tol)); + //if (!sqrt_information_) return true; // ALEX todo; + return equal_with_abs_tol(R(), p->R(), sqrt(tol)); } Vector Gaussian::whiten(const Vector& v) const { - return sqrt_information_ * v; + return thisR() * v; } Vector Gaussian::unwhiten(const Vector& v) const { - return backSubstituteUpper(sqrt_information_, v); + return backSubstituteUpper(thisR(), v); } double Gaussian::Mahalanobis(const Vector& v) const { @@ -114,11 +115,11 @@ namespace gtsam { } Matrix Gaussian::Whiten(const Matrix& H) const { - return sqrt_information_ * H; + return thisR() * H; } void Gaussian::WhitenInPlace(Matrix& H) const { - H = sqrt_information_ * H; + H = thisR() * H; } // General QR, see also special version in Constrained @@ -140,8 +141,8 @@ namespace gtsam { /* ************************************************************************* */ // TODO: can we avoid calling reciprocal twice ? Diagonal::Diagonal(const Vector& sigmas) : - Gaussian(diag(reciprocal(sigmas))), invsigmas_(reciprocal(sigmas)), - sigmas_(sigmas) { + Gaussian(sigmas.size()), invsigmas_(reciprocal(sigmas)), sigmas_( + sigmas) { } Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { diff --git a/cpp/NoiseModel.h b/cpp/NoiseModel.h index 044af4146..8e6bad434 100644 --- a/cpp/NoiseModel.h +++ b/cpp/NoiseModel.h @@ -74,15 +74,26 @@ namespace gtsam { */ struct Gaussian: public Base { - protected: + private: // TODO: store as boost upper-triangular or whatever is passed from above /* Matrix square root of information matrix (R) */ - Matrix sqrt_information_; + boost::optional sqrt_information_; + + /** + * Return R itself, but note that Whiten(H) is cheaper than R*H + */ + const Matrix& thisR() const { + // should never happen + if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R matrix"); + return *sqrt_information_; + } + + protected: /** protected constructor takes square root information matrix */ - Gaussian(const Matrix& sqrt_information) : - Base(sqrt_information.size1()), sqrt_information_(sqrt_information) { + Gaussian(size_t dim, const boost::optional& sqrt_information = boost::none) : + Base(dim), sqrt_information_(sqrt_information) { } public: @@ -94,7 +105,7 @@ namespace gtsam { * @param smart, check if can be simplified to derived class */ static shared_ptr SqrtInformation(const Matrix& R) { - return shared_ptr(new Gaussian(R)); + return shared_ptr(new Gaussian(R.size1(),R)); } /** @@ -107,7 +118,7 @@ namespace gtsam { * A Gaussian noise model created by specifying an information matrix. */ static shared_ptr Information(const Matrix& Q) { - return shared_ptr(new Gaussian(square_root_positive(Q))); + return shared_ptr(new Gaussian(Q.size1(),square_root_positive(Q))); } virtual void print(const std::string& name) const; @@ -151,9 +162,7 @@ namespace gtsam { /** * Return R itself, but note that Whiten(H) is cheaper than R*H */ - const Matrix& R() const { - return sqrt_information_; - } + virtual Matrix R() const { return thisR();} }; // Gaussian @@ -223,6 +232,12 @@ namespace gtsam { */ virtual Vector sample() const; + /** + * Return R itself, but note that Whiten(H) is cheaper than R*H + */ + virtual Matrix R() const { + return diag(invsigmas_); + } }; // Diagonal