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 {
/* ************************************************************************* */
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<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 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

28
gtsam/linear/GaussianFactorGraphUnordered.h
View File

@ -91,6 +91,13 @@ namespace gtsam {
template<class DERIVEDFACTOR>
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 */
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