12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Enabled some linear algebra functions in GFG

release/4.3a0
Richard Roberts 2013-07-16 20:22:08 +00:00
parent 223f89d931
commit e846ed119c
2 changed files with 84 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
gtsam/linear/GaussianFactorGraphUnordered.cpp
View File

@ -34,6 +34,12 @@ using namespace gtsam;
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) void GaussianFactorGraphUnordered::push_back_bayesTree(const GaussianBayesTreeUnordered& bayesTree)
{ {
@ -201,14 +207,14 @@ namespace gtsam {
return BaseEliminateable::eliminateMultifrontal(function)->optimize(); return BaseEliminateable::eliminateMultifrontal(function)->optimize();
} }
///* ************************************************************************* */ /* ************************************************************************* */
//VectorValuesUnordered GaussianFactorGraphUnordered::gradient(const VectorValuesUnordered& x0) const VectorValuesUnordered GaussianFactorGraphUnordered::gradient(const VectorValuesUnordered& x0) const
//{ {
// VectorValuesUnordered g = VectorValuesUnordered::Zero(x0); VectorValuesUnordered g = VectorValuesUnordered::Zero(x0);
// Errors e = gaussianErrors(*this, x0); Errors e = gaussianErrors(x0);
// transposeMultiplyAdd(*this, 1.0, e, g); transposeMultiplyAdd(1.0, e, g);
// return g; return g;
//} }
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
@ -221,20 +227,19 @@ namespace gtsam {
} }
} }
///* ************************************************************************* */ /* ************************************************************************* */
//VectorValuesUnordered GaussianFactorGraphUnordered::gradientAtZero() const VectorValuesUnordered GaussianFactorGraphUnordered::gradientAtZero() const
//{ {
// assert(false); // Zero-out the gradient
// // Zero-out the gradient VectorValuesUnordered g;
// VectorValuesUnordered g; Errors e;
// Errors e; BOOST_FOREACH(const sharedFactor& Ai_G, *this) {
// BOOST_FOREACH(const sharedFactor& Ai_G, *this) { JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
// JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); e.push_back(-Ai->getb());
// e.push_back(-Ai->getb()); }
// } transposeMultiplyAdd(1.0, e, g);
// transposeMultiplyAdd(*this, 1.0, e, g); return g;
// return g; }
//}
/* ************************************************************************* */ /* ************************************************************************* */
bool hasConstraints(const GaussianFactorGraphUnordered& factors) { bool hasConstraints(const GaussianFactorGraphUnordered& factors) {
@ -295,16 +300,17 @@ namespace gtsam {
// } // }
//} //}
///* ************************************************************************* */ /* ************************************************************************* */
//// x += alpha*A'*e // x += alpha*A'*e
//void transposeMultiplyAdd(const GaussianFactorGraphUnordered& fg, double alpha, const Errors& e, VectorValuesUnordered& x) { void GaussianFactorGraphUnordered::transposeMultiplyAdd(double alpha, const Errors& e, VectorValuesUnordered& x) const
// // For each factor add the gradient contribution {
// Errors::const_iterator ei = e.begin(); // For each factor add the gradient contribution
// BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) { Errors::const_iterator ei = e.begin();
// JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); BOOST_FOREACH(const sharedFactor& Ai_G, *this) {
// Ai->transposeMultiplyAdd(alpha,*(ei++),x); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
// } Ai->transposeMultiplyAdd(alpha, *(ei++), x);
//} }
}
///* ************************************************************************* */ ///* ************************************************************************* */
//void residual(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r) { //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) { VectorValuesUnordered GaussianFactorGraphUnordered::transposeMultiply(const Errors& e) const
// x.setZero(); {
// Key i = 0; VectorValuesUnordered x;
// BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) { Errors::const_iterator ei = e.begin();
// JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); BOOST_FOREACH(const sharedFactor& Ai_G, *this) {
// for(JacobianFactorUnordered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
// x[*j] += Ai->getA(j).transpose() * r[i]; for(JacobianFactorUnordered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
// } // Create the value as a zero vector if it does not exist.
// ++i; pair<VectorValuesUnordered::iterator, bool> 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 GaussianFactorGraphUnordered::gaussianErrors(const VectorValuesUnordered& x) const
// Errors e; {
// BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) { Errors e;
// JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); BOOST_FOREACH(const sharedFactor& Ai_G, *this) {
// e.push_back(Ai->error_vector(x)); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
// } e.push_back(Ai->error_vector(x));
// return e; }
//} return e;
}
} // namespace gtsam } // namespace gtsam

28
gtsam/linear/GaussianFactorGraphUnordered.h
View File

@ -91,6 +91,13 @@ namespace gtsam {
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
GaussianFactorGraphUnordered(const FactorGraphUnordered<DERIVEDFACTOR>& graph) : Base(graph) {} GaussianFactorGraphUnordered(const FactorGraphUnordered<DERIVEDFACTOR>& graph) : Base(graph) {}
/// @name Testable
/// @{
bool equals(const This& fg, double tol = 1e-9) const;
/// @}
/** Add a factor by value - makes a copy */ /** Add a factor by value - makes a copy */
void add(const GaussianFactorUnordered& factor) { factors_.push_back(factor.clone()); } 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 * @param x0 The center about which to compute the gradient
* @return The gradient as a VectorValuesUnordered * @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 * 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, * @param [output] g A VectorValuesUnordered to store the gradient, which must be preallocated,
* see allocateVectorValues * see allocateVectorValues
* @return The gradient as a VectorValuesUnordered */ * @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 /** 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$. * 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] */ * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] */
//VectorValuesUnordered optimizeGradientSearch() const; //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: private:
@ -327,16 +343,8 @@ namespace gtsam {
///** In-place version e <- A*x that takes an iterator. */ ///** In-place version e <- A*x that takes an iterator. */
//GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x, const Errors::iterator& e); //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 */ ///* matrix-vector operations */
//GTSAM_EXPORT void residual(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r); //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 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 } // namespace gtsam