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

move linear algebra functions for jacobian factor graph to a new file

release/4.3a0
Yong-Dian Jian 2012-06-07 01:24:19 +00:00
parent c518381373
commit a9c36fc172
10 changed files with 184 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/linear/GaussianBayesNet.cpp
View File

@ -19,7 +19,7 @@
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/VectorValues.h>

2
gtsam/linear/GaussianBayesTree.cpp
View File

@ -18,7 +18,7 @@
*/
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/JacobianFactorGraph.h>
namespace gtsam {

94
gtsam/linear/JacobianFactor.cpp
View File

@ -536,98 +536,4 @@ namespace gtsam {
model_ = noiseModel::Diagonal::Sigmas(sigmas);
}
/* ************************************************************************* */
Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x) {
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
e.push_back((*Ai)*x);
}
return e;
}
/* ************************************************************************* */
void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, Errors& e) {
multiplyInPlace(fg,x,e.begin());
}
/* ************************************************************************* */
void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const Errors::iterator& e) {
Errors::iterator ei = e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
*ei = (*Ai)*x;
ei++;
}
}
/* ************************************************************************* */
// x += alpha*A'*e
void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& 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) {
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
}
}
/* ************************************************************************* */
VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x0) {
// It is crucial for performance to make a zero-valued clone of x
VectorValues g = VectorValues::Zero(x0);
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
e.push_back(factor->error_vector(x0));
}
transposeMultiplyAdd(fg, 1.0, e, g);
return g;
}
/* ************************************************************************* */
void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g) {
// Zero-out the gradient
g.setZero();
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
e.push_back(-factor->getb());
}
transposeMultiplyAdd(fg, 1.0, e, g);
}
/* ************************************************************************* */
void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
Index i = 0 ;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
r[i] = factor->getb();
i++;
}
VectorValues Ax = VectorValues::SameStructure(r);
multiply(fg,x,Ax);
axpy(-1.0,Ax,r);
}
/* ************************************************************************* */
void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
r.vector() = Vector::Zero(r.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
SubVector &y = r[i];
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
y += factor->getA(j) * x[*j];
}
++i;
}
}
/* ************************************************************************* */
void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x) {
x.vector() = Vector::Zero(x.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
x[*j] += factor->getA(j).transpose() * r[i];
}
++i;
}
}
}

40
gtsam/linear/JacobianFactor.h
View File

@ -318,45 +318,5 @@ namespace gtsam {
}
}; // JacobianFactor
/** return A*x */
Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x);
/* In-place version e <- A*x that overwrites e. */
void multiplyInPlace(const FactorGraph<JacobianFactor>& 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);
/** x += alpha*A'*e */
void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, const Errors& e, VectorValues& x);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around \f$ x = x_0 \f$.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @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);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around zero.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @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);
// 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);
} // gtsam

108
gtsam/linear/JacobianFactorGraph.cpp Normal file
View File

@ -0,0 +1,108 @@
/*
* JacobianFactorGraph.cpp
*
* Created on: Jun 6, 2012
* Author: ydjian
*/
#include <gtsam/linear/JacobianFactorGraph.h>
#include <boost/foreach.hpp>
namespace gtsam {
/* ************************************************************************* */
Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x) {
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
e.push_back((*Ai)*x);
}
return e;
}
/* ************************************************************************* */
void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, Errors& e) {
multiplyInPlace(fg,x,e.begin());
}
/* ************************************************************************* */
void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const Errors::iterator& e) {
Errors::iterator ei = e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
*ei = (*Ai)*x;
ei++;
}
}
/* ************************************************************************* */
// x += alpha*A'*e
void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& 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) {
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
}
}
/* ************************************************************************* */
VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x0) {
// It is crucial for performance to make a zero-valued clone of x
VectorValues g = VectorValues::Zero(x0);
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
e.push_back(factor->error_vector(x0));
}
transposeMultiplyAdd(fg, 1.0, e, g);
return g;
}
/* ************************************************************************* */
void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g) {
// Zero-out the gradient
g.setZero();
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
e.push_back(-factor->getb());
}
transposeMultiplyAdd(fg, 1.0, e, g);
}
/* ************************************************************************* */
void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
Index i = 0 ;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
r[i] = factor->getb();
i++;
}
VectorValues Ax = VectorValues::SameStructure(r);
multiply(fg,x,Ax);
axpy(-1.0,Ax,r);
}
/* ************************************************************************* */
void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) {
r.vector() = Vector::Zero(r.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
SubVector &y = r[i];
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
y += factor->getA(j) * x[*j];
}
++i;
}
}
/* ************************************************************************* */
void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x) {
x.vector() = Vector::Zero(x.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
x[*j] += factor->getA(j).transpose() * r[i];
}
++i;
}
}
}

70
gtsam/linear/JacobianFactorGraph.h Normal file
View File

@ -0,0 +1,70 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* JacobianFactorGraph.cpp
*
* Created on: Jun 6, 2012
* Author: ydjian
*/
#pragma once
#include <gtsam/linear/Errors.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam {
/**
* Linear Algebra Operations for the JacobianFactorGraph
*/
/** return A*x */
Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x);
/** In-place version e <- A*x that overwrites e. */
void multiplyInPlace(const FactorGraph<JacobianFactor>& 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);
/** x += alpha*A'*e */
void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, const Errors& e, VectorValues& x);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around \f$ x = x_0 \f$.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @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);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around zero.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @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);
/* 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);
}

2
gtsam/linear/SimpleSPCGSolver.cpp
View File

@ -13,7 +13,7 @@
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/EliminationTree.h>
#include <boost/foreach.hpp>

1
gtsam/nonlinear/ISAM2.cpp
View File

@ -26,6 +26,7 @@ using namespace boost::assign;
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>

1
tests/testGaussianFactorGraphB.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Matrix.h>

1
tests/testGaussianISAM2.cpp
View File

@ -10,6 +10,7 @@
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/smallExample.h>
#include <gtsam/slam/planarSLAM.h>