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

20
gtsam/linear/JacobianFactorGraph.h
View File

@ -24,17 +24,19 @@
namespace gtsam { namespace gtsam {
typedef FactorGraph<JacobianFactor> JacobianFactorGraph;
/** return A*x */ /** 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. */ /** 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. */ /** 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 */ /** 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, * Compute the gradient of the energy function,
@ -45,7 +47,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 VectorValues * @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, * 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 * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues * @return The gradient as a VectorValues
*/ */
void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g); void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g);
/* matrix-vector operations */ /* matrix-vector operations */
void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r); void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r);
void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r); void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r);
void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x); void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x);
} }