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

typedef - though this double inheritance chain is a mess. Should GaussianFactorGraph be a typedef as well?

release/4.3a0
Frank Dellaert 2012-06-09 18:51:43 +00:00
parent 5b08dde1f3
commit 03465500e9
2 changed files with 21 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/linear/JacobianFactorGraph.cpp
View File

@ -10,7 +10,7 @@
namespace gtsam {
/* ************************************************************************* */
Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x) {
Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x) {
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
e.push_back((*Ai)*x);
@ -19,12 +19,12 @@ Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x) {
}
/* ************************************************************************* */
void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, Errors& e) {
void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e) {
multiplyInPlace(fg,x,e.begin());
}
/* ************************************************************************* */
void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const Errors::iterator& e) {
void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) {
Errors::iterator ei = e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
*ei = (*Ai)*x;
@ -35,7 +35,7 @@ void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues&
/* ************************************************************************* */
// x += alpha*A'*e
void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, const Errors& e, VectorValues& x) {
void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) {
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
@ -44,7 +44,7 @@ void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, c
}
/* ************************************************************************* */
VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x0) {
VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0) {
// It is crucial for performance to make a zero-valued clone of x
VectorValues g = VectorValues::Zero(x0);
Errors e;
@ -56,7 +56,7 @@ VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues&
}
/* ************************************************************************* */
void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g) {
void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g) {
// Zero-out the gradient
g.setZero();
Errors e;
@ -67,7 +67,7 @@ void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g) {
}
/* ************************************************************************* */
void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
Index i = 0 ;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
r[i] = factor->getb();
@ -79,7 +79,7 @@ void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, Vect
}
/* ************************************************************************* */
void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
r.vector() = Vector::Zero(r.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
@ -92,7 +92,7 @@ void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, Vect
}
/* ************************************************************************* */
void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x) {
void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x) {
x.vector() = Vector::Zero(x.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
@ -102,6 +102,6 @@ void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues
++i;
}
}
}
} // namespace

20
gtsam/linear/JacobianFactorGraph.h
View File

@ -24,17 +24,19 @@
namespace gtsam {
typedef FactorGraph<JacobianFactor> JacobianFactorGraph;
/** return A*x */
Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x);
Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x);
/** In-place version e <- A*x that overwrites e. */
void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, Errors& e);
void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e);
/** In-place version e <- A*x that takes an iterator. */
void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const Errors::iterator& e);
void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e);
/** x += alpha*A'*e */
void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, const Errors& e, VectorValues& x);
void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x);
/**
* Compute the gradient of the energy function,
@ -45,7 +47,7 @@ namespace gtsam {
* @param x0 The center about which to compute the gradient
* @return The gradient as a VectorValues
*/
VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x0);
VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0);
/**
* Compute the gradient of the energy function,
@ -56,11 +58,11 @@ namespace gtsam {
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues
*/
void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g);
void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g);
/* matrix-vector operations */
void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r);
void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r);
void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x);
void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r);
void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r);
void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x);
}