From e846ed119c6742e89d18584f9b67c395e8befa4d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 16 Jul 2013 20:22:08 +0000 Subject: [PATCH] Enabled some linear algebra functions in GFG --- gtsam/linear/GaussianFactorGraphUnordered.cpp | 119 ++++++++++-------- gtsam/linear/GaussianFactorGraphUnordered.h | 28 +++-- 2 files changed, 84 insertions(+), 63 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraphUnordered.cpp b/gtsam/linear/GaussianFactorGraphUnordered.cpp index 5d0c77ab0..9f0cd937a 100644 --- a/gtsam/linear/GaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/GaussianFactorGraphUnordered.cpp @@ -34,6 +34,12 @@ using namespace gtsam; namespace gtsam { + /* ************************************************************************* */ + bool GaussianFactorGraphUnordered::equals(const This& fg, double tol) const + { + return Base::equals(fg, tol); + } + /* ************************************************************************* */ void GaussianFactorGraphUnordered::push_back_bayesTree(const GaussianBayesTreeUnordered& bayesTree) { @@ -201,14 +207,14 @@ namespace gtsam { return BaseEliminateable::eliminateMultifrontal(function)->optimize(); } - ///* ************************************************************************* */ - //VectorValuesUnordered GaussianFactorGraphUnordered::gradient(const VectorValuesUnordered& x0) const - //{ - // VectorValuesUnordered g = VectorValuesUnordered::Zero(x0); - // Errors e = gaussianErrors(*this, x0); - // transposeMultiplyAdd(*this, 1.0, e, g); - // return g; - //} + /* ************************************************************************* */ + VectorValuesUnordered GaussianFactorGraphUnordered::gradient(const VectorValuesUnordered& x0) const + { + VectorValuesUnordered g = VectorValuesUnordered::Zero(x0); + Errors e = gaussianErrors(x0); + transposeMultiplyAdd(1.0, e, g); + return g; + } /* ************************************************************************* */ namespace { @@ -221,20 +227,19 @@ namespace gtsam { } } - ///* ************************************************************************* */ - //VectorValuesUnordered GaussianFactorGraphUnordered::gradientAtZero() const - //{ - // assert(false); - // // Zero-out the gradient - // VectorValuesUnordered g; - // Errors e; - // BOOST_FOREACH(const sharedFactor& Ai_G, *this) { - // JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - // e.push_back(-Ai->getb()); - // } - // transposeMultiplyAdd(*this, 1.0, e, g); - // return g; - //} + /* ************************************************************************* */ + VectorValuesUnordered GaussianFactorGraphUnordered::gradientAtZero() const + { + // Zero-out the gradient + VectorValuesUnordered g; + Errors e; + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e.push_back(-Ai->getb()); + } + transposeMultiplyAdd(1.0, e, g); + return g; + } /* ************************************************************************* */ bool hasConstraints(const GaussianFactorGraphUnordered& factors) { @@ -295,16 +300,17 @@ namespace gtsam { // } //} - ///* ************************************************************************* */ - //// x += alpha*A'*e - //void transposeMultiplyAdd(const GaussianFactorGraphUnordered& fg, double alpha, const Errors& e, VectorValuesUnordered& x) { - // // For each factor add the gradient contribution - // Errors::const_iterator ei = e.begin(); - // BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) { - // JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - // Ai->transposeMultiplyAdd(alpha,*(ei++),x); - // } - //} + /* ************************************************************************* */ + // x += alpha*A'*e + void GaussianFactorGraphUnordered::transposeMultiplyAdd(double alpha, const Errors& e, VectorValuesUnordered& x) const + { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + Ai->transposeMultiplyAdd(alpha, *(ei++), x); + } + } ///* ************************************************************************* */ //void residual(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r) { @@ -333,27 +339,34 @@ namespace gtsam { // } //} - ///* ************************************************************************* */ - //void transposeMultiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &r, VectorValuesUnordered &x) { - // x.setZero(); - // Key i = 0; - // BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) { - // JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - // for(JacobianFactorUnordered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { - // x[*j] += Ai->getA(j).transpose() * r[i]; - // } - // ++i; - // } - //} + /* ************************************************************************* */ + VectorValuesUnordered GaussianFactorGraphUnordered::transposeMultiply(const Errors& e) const + { + VectorValuesUnordered x; + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + for(JacobianFactorUnordered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { + // Create the value as a zero vector if it does not exist. + pair xi = x.tryInsert(*j, Vector()); + if(xi.second) + xi.first->second = Vector::Zero(Ai->getDim(j)); + xi.first->second += Ai->getA(j).transpose() * *ei; + } + ++ ei; + } + return x; + } - ///* ************************************************************************* */ - //Errors gaussianErrors(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x) { - // Errors e; - // BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) { - // JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - // e.push_back(Ai->error_vector(x)); - // } - // return e; - //} + /* ************************************************************************* */ + Errors GaussianFactorGraphUnordered::gaussianErrors(const VectorValuesUnordered& x) const + { + Errors e; + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e.push_back(Ai->error_vector(x)); + } + return e; + } } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraphUnordered.h b/gtsam/linear/GaussianFactorGraphUnordered.h index 892caf402..705ae74d8 100644 --- a/gtsam/linear/GaussianFactorGraphUnordered.h +++ b/gtsam/linear/GaussianFactorGraphUnordered.h @@ -91,6 +91,13 @@ namespace gtsam { template GaussianFactorGraphUnordered(const FactorGraphUnordered& graph) : Base(graph) {} + /// @name Testable + /// @{ + + bool equals(const This& fg, double tol = 1e-9) const; + + /// @} + /** Add a factor by value - makes a copy */ void add(const GaussianFactorUnordered& factor) { factors_.push_back(factor.clone()); } @@ -218,7 +225,7 @@ namespace gtsam { * @param x0 The center about which to compute the gradient * @return The gradient as a VectorValuesUnordered */ - //VectorValuesUnordered gradient(const VectorValuesUnordered& x0) const; + VectorValuesUnordered gradient(const VectorValuesUnordered& x0) const; /** * Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b @@ -227,7 +234,7 @@ namespace gtsam { * @param [output] g A VectorValuesUnordered to store the gradient, which must be preallocated, * see allocateVectorValues * @return The gradient as a VectorValuesUnordered */ - //VectorValuesUnordered gradientAtZero() const; + VectorValuesUnordered gradientAtZero() const; /** Optimize along the gradient direction, with a closed-form computation to perform the line * search. The gradient is computed about \f$ \delta x=0 \f$. @@ -255,6 +262,15 @@ namespace gtsam { * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] */ //VectorValuesUnordered optimizeGradientSearch() const; + /** x = A'*e */ + VectorValuesUnordered transposeMultiply(const Errors& e) const; + + /** x += alpha*A'*e */ + void transposeMultiplyAdd(double alpha, const Errors& e, VectorValuesUnordered& x) const; + + /** return A*x-b */ + Errors gaussianErrors(const VectorValuesUnordered& x) const; + /// @} private: @@ -327,16 +343,8 @@ namespace gtsam { ///** In-place version e <- A*x that takes an iterator. */ //GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x, const Errors::iterator& e); - ///** x += alpha*A'*e */ - //GTSAM_EXPORT void transposeMultiplyAdd(const GaussianFactorGraphUnordered& fg, double alpha, const Errors& e, VectorValuesUnordered& x); - ///* matrix-vector operations */ //GTSAM_EXPORT void residual(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r); //GTSAM_EXPORT void multiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r); - //GTSAM_EXPORT void transposeMultiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &r, VectorValuesUnordered &x); - - ///** return A*x-b - // * \todo Make this a member function - affects SubgraphPreconditioner */ - //GTSAM_EXPORT Errors gaussianErrors(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x); } // namespace gtsam