diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 99802a547..348ef5fc3 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -145,55 +146,22 @@ namespace gtsam { } /* ************************************************************************* */ - //Matrix GaussianFactorGraph::augmentedHessian() const { - // // combine all factors and get upper-triangular part of Hessian - // HessianFactor combined(*this); - // Matrix result = combined.info(); - // // Fill in lower-triangular part of Hessian - // result.triangularView() = result.transpose(); - // return result; - //} + Matrix GaussianFactorGraph::augmentedHessian() const { + // combine all factors and get upper-triangular part of Hessian + HessianFactor combined(*this); + Matrix result = combined.info(); + // Fill in lower-triangular part of Hessian + result.triangularView() = result.transpose(); + return result; + } /* ************************************************************************* */ - //std::pair GaussianFactorGraph::hessian() const { - // Matrix augmented = augmentedHessian(); - // return make_pair( - // augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1), - // augmented.col(augmented.rows()-1).head(augmented.rows()-1)); - //} - - /* ************************************************************************* */ - //GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< - // GaussianFactor>& factors, size_t nrFrontals) { - - // const bool debug = ISDEBUG("EliminateCholesky"); - - // // Form Ab' * Ab - // gttic(combine); - // HessianFactor::shared_ptr combinedFactor(new HessianFactor(factors)); - // gttoc(combine); - - // // Do Cholesky, note that after this, the lower triangle still contains - // // some untouched non-zeros that should be zero. We zero them while - // // extracting submatrices next. - // gttic(partial_Cholesky); - // combinedFactor->partialCholesky(nrFrontals); - - // gttoc(partial_Cholesky); - - // // Extract conditional and fill in details of the remaining factor - // gttic(split); - // GaussianConditional::shared_ptr conditional = - // combinedFactor->splitEliminatedFactor(nrFrontals); - // if (debug) { - // conditional->print("Extracted conditional: "); - // combinedFactor->print("Eliminated factor (L piece): "); - // } - // gttoc(split); - - // combinedFactor->assertInvariants(); - // return make_pair(conditional, combinedFactor); - //} + std::pair GaussianFactorGraph::hessian() const { + Matrix augmented = augmentedHessian(); + return make_pair( + augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1), + augmented.col(augmented.rows()-1).head(augmented.rows()-1)); + } /* ************************************************************************* */ VectorValues GaussianFactorGraph::optimize(OptionalOrdering ordering, const Eliminate& function) const @@ -236,6 +204,31 @@ namespace gtsam { return g; } + /* ************************************************************************* */ + Errors GaussianFactorGraph::operator*(const VectorValues& x) const { + Errors e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + e.push_back((*Ai) * x); + } + return e; + } + + /* ************************************************************************* */ + void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const { + multiplyInPlace(x, e.begin()); + } + + /* ************************************************************************* */ + void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const { + Errors::iterator ei = e; + BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + *ei = (*Ai)*x; + ei++; + } + } + /* ************************************************************************* */ bool hasConstraints(const GaussianFactorGraph& factors) { typedef JacobianFactor J; @@ -248,53 +241,6 @@ namespace gtsam { return false; } - /* ************************************************************************* */ - //GaussianFactorGraph::EliminationResult EliminatePreferCholesky( - // const FactorGraph& factors, size_t nrFrontals) { - - // // If any JacobianFactors have constrained noise models, we have to convert - // // all factors to JacobianFactors. Otherwise, we can convert all factors - // // to HessianFactors. This is because QR can handle constrained noise - // // models but Cholesky cannot. - // if (hasConstraints(factors)) - // return EliminateQR(factors, nrFrontals); - // else { - // GaussianFactorGraph::EliminationResult ret; - // gttic(EliminateCholesky); - // ret = EliminateCholesky(factors, nrFrontals); - // gttoc(EliminateCholesky); - // return ret; - // } - - //} // \EliminatePreferCholesky - - - - ///* ************************************************************************* */ - //Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) { - // Errors e; - // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - // e.push_back((*Ai)*x); - // } - // return e; - //} - - ///* ************************************************************************* */ - //void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e) { - // multiplyInPlace(fg,x,e.begin()); - //} - - ///* ************************************************************************* */ - //void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) { - // Errors::iterator ei = e; - // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - // *ei = (*Ai)*x; - // ei++; - // } - //} - /* ************************************************************************* */ // x += alpha*A'*e void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 6c9cf9f23..7b1d765b3 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -193,7 +193,7 @@ namespace gtsam { and the negative log-likelihood is \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$. */ - //Matrix augmentedHessian() const; + Matrix augmentedHessian() const; /** * Return the dense Hessian \f$ \Lambda \f$ and information vector @@ -201,7 +201,7 @@ namespace gtsam { * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also * GaussianFactorGraph::augmentedHessian. */ - //std::pair hessian() const; + std::pair hessian() const; /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using * the dense elimination function specified in \c function (default EliminatePreferCholesky), @@ -254,7 +254,7 @@ namespace gtsam { * \f$ G \f$, returning * * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] */ - //VectorValues optimizeGradientSearch() const; + VectorValues optimizeGradientSearch() const; /** x = A'*e */ VectorValues transposeMultiply(const Errors& e) const; @@ -265,6 +265,15 @@ namespace gtsam { /** return A*x-b */ Errors gaussianErrors(const VectorValues& x) const; + ///** return A*x */ + Errors operator*(const VectorValues& x) const; + + ///** In-place version e <- A*x that overwrites e. */ + void multiplyInPlace(const VectorValues& x, Errors& e) const; + + /** In-place version e <- A*x that takes an iterator. */ + void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const; + /// @} private: @@ -283,60 +292,8 @@ namespace gtsam { */ GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors); - /** - * Densely partially eliminate with Cholesky factorization. JacobianFactors - * are left-multiplied with their transpose to form the Hessian using the - * conversion constructor HessianFactor(const JacobianFactor&). - * - * If any factors contain constrained noise models (any sigmas equal to - * zero), QR factorization will be performed instead, because our current - * implementation cannot handle constrained noise models in Cholesky - * factorization. EliminateCholesky(), on the other hand, will fail if any - * factors contain constrained noise models. - * - * Variables are eliminated in the natural order of the variable indices of in - * the factors. - * @param factors Factors to combine and eliminate - * @param nrFrontals Number of frontal variables to eliminate. - * @return The conditional and remaining factor - - * \addtogroup LinearSolving - */ - //GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminatePreferCholesky(const FactorGraph< - // GaussianFactor>& factors, size_t nrFrontals = 1); - - /** - * Densely partially eliminate with Cholesky factorization. JacobianFactors - * are left-multiplied with their transpose to form the Hessian using the - * conversion constructor HessianFactor(const JacobianFactor&). - * - * If any factors contain constrained noise models, this function will fail - * because our current implementation cannot handle constrained noise models - * in Cholesky factorization. The function EliminatePreferCholesky() - * automatically does QR instead when this is the case. - * - * Variables are eliminated in the natural order of the variable indices of in - * the factors. - * @param factors Factors to combine and eliminate - * @param nrFrontals Number of frontal variables to eliminate. - * @return The conditional and remaining factor - - * \addtogroup LinearSolving - */ - //GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< - // GaussianFactor>& factors, size_t nrFrontals = 1); - /****** Linear Algebra Opeations ******/ - ///** return A*x */ - //GTSAM_EXPORT Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x); - - ///** In-place version e <- A*x that overwrites e. */ - //GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e); - - ///** In-place version e <- A*x that takes an iterator. */ - //GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e); - ///* matrix-vector operations */ //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);