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

Made iterative compile

release/4.3a0
Richard Roberts 2013-08-05 22:31:05 +00:00
parent 114f389eeb
commit 73fe5b2be4
8 changed files with 128 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -15,10 +15,9 @@
* @author: Frank Dellaert * @author: Frank Dellaert
*/ */
#if 0
#include <gtsam/linear/SubgraphPreconditioner.h> #include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
using namespace std; using namespace std;
@ -41,47 +40,47 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
const sharedBayesNet& Rc1, const sharedValues& xbar) : const sharedBayesNet& Rc1, const sharedValues& xbar) :
Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) { Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// x = xbar + inv(R1)*y // x = xbar + inv(R1)*y
VectorValues SubgraphPreconditioner::x(const VectorValues& y) const { VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
return *xbar_ + gtsam::backSubstitute(*Rc1_, y); return *xbar_ + Rc1_->backSubstitute(y);
} }
/* ************************************************************************* */ /* ************************************************************************* */
double error(const SubgraphPreconditioner& sp, const VectorValues& y) { double SubgraphPreconditioner::error(const VectorValues& y) const {
Errors e(y); Errors e(y);
VectorValues x = sp.x(y); VectorValues x = this->x(y);
Errors e2 = gaussianErrors(*sp.Ab2(),x); Errors e2 = Ab2()->gaussianErrors(x);
return 0.5 * (dot(e, e) + dot(e2,e2)); return 0.5 * (dot(e, e) + dot(e2,e2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y) { VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
VectorValues x = gtsam::backSubstitute(*sp.Rc1(), y); /* inv(R1)*y */ VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */
Errors e = (*sp.Ab2()*x - *sp.b2bar()); /* (A2*inv(R1)*y-b2bar) */ Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */
VectorValues v = VectorValues::Zero(x); VectorValues v = VectorValues::Zero(x);
transposeMultiplyAdd(*sp.Ab2(), 1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */
return y + gtsam::backSubstituteTranspose(*sp.Rc1(), v); return y + Rc1()->backSubstituteTranspose(v);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors operator*(const SubgraphPreconditioner& sp, const VectorValues& y) { Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
Errors e(y); Errors e(y);
VectorValues x = gtsam::backSubstitute(*sp.Rc1(), y); /* x=inv(R1)*y */ VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */
Errors e2 = *sp.Ab2() * x; /* A2*x */ Errors e2 = *Ab2() * x; /* A2*x */
e.splice(e.end(), e2); e.splice(e.end(), e2);
return e; return e;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// In-place version that overwrites e // In-place version that overwrites e
void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e) { void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
Errors::iterator ei = e.begin(); Errors::iterator ei = e.begin();
for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) { for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) {
@ -89,33 +88,33 @@ namespace gtsam {
} }
// Add A2 contribution // Add A2 contribution
VectorValues x = gtsam::backSubstitute(*sp.Rc1(), y); // x=inv(R1)*y VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y
gtsam::multiplyInPlace(*sp.Ab2(), x, ei); // use iterator version Ab2()->multiplyInPlace(x, ei); // use iterator version
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
VectorValues operator^(const SubgraphPreconditioner& sp, const Errors& e) { VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
Errors::const_iterator it = e.begin(); Errors::const_iterator it = e.begin();
VectorValues y = sp.zero(); VectorValues y = zero();
for ( Index i = 0 ; i < y.size() ; ++i, ++it ) for ( Index i = 0 ; i < y.size() ; ++i, ++it )
y[i] = *it ; y[i] = *it ;
sp.transposeMultiplyAdd2(1.0,it,e.end(),y); transposeMultiplyAdd2(1.0,it,e.end(),y);
return y; return y;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// y += alpha*A'*e // y += alpha*A'*e
void transposeMultiplyAdd void SubgraphPreconditioner::transposeMultiplyAdd
(const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y) { (double alpha, const Errors& e, VectorValues& y) const {
Errors::const_iterator it = e.begin(); Errors::const_iterator it = e.begin();
for ( Index i = 0 ; i < y.size() ; ++i, ++it ) { for ( Index i = 0 ; i < y.size() ; ++i, ++it ) {
const Vector& ei = *it; const Vector& ei = *it;
axpy(alpha, ei, y[i]); axpy(alpha, ei, y[i]);
} }
sp.transposeMultiplyAdd2(alpha, it, e.end(), y); transposeMultiplyAdd2(alpha, it, e.end(), y);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -129,8 +128,8 @@ namespace gtsam {
while (it != end) e2.push_back(*(it++)); while (it != end) e2.push_back(*(it++));
VectorValues x = VectorValues::Zero(y); // x = 0 VectorValues x = VectorValues::Zero(y); // x = 0
gtsam::transposeMultiplyAdd(*Ab2_,1.0,e2,x); // x += A2'*e2 Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2
axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -139,5 +138,3 @@ namespace gtsam {
Ab2_->print(); Ab2_->print();
} }
} // nsamespace gtsam } // nsamespace gtsam
#endif

24
gtsam/linear/SubgraphPreconditioner.h
View File

@ -23,8 +23,6 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#if 0
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
@ -66,6 +64,9 @@ namespace gtsam {
*/ */
SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar);
/** print the object */
void print(const std::string& s = "SubgraphPreconditioner") const;
/** Access Ab2 */ /** Access Ab2 */
const sharedFG& Ab2() const { return Ab2_; } const sharedFG& Ab2() const { return Ab2_; }
@ -98,31 +99,26 @@ namespace gtsam {
void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin, void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin,
Errors::const_iterator end, VectorValues& y) const; Errors::const_iterator end, VectorValues& y) const;
/** print the object */
void print(const std::string& s = "SubgraphPreconditioner") const;
};
/* error, given y */ /* error, given y */
GTSAM_EXPORT double error(const SubgraphPreconditioner& sp, const VectorValues& y); double error(const VectorValues& y) const;
/** gradient = y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar) */ /** gradient = y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar) */
GTSAM_EXPORT VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y); VectorValues gradient(const VectorValues& y) const;
/** Apply operator A */ /** Apply operator A */
GTSAM_EXPORT Errors operator*(const SubgraphPreconditioner& sp, const VectorValues& y); Errors operator*(const VectorValues& y) const;
/** Apply operator A in place: needs e allocated already */ /** Apply operator A in place: needs e allocated already */
GTSAM_EXPORT void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e); void multiplyInPlace(const VectorValues& y, Errors& e) const;
/** Apply operator A' */ /** Apply operator A' */
GTSAM_EXPORT VectorValues operator^(const SubgraphPreconditioner& sp, const Errors& e); VectorValues operator^(const Errors& e) const;
/** /**
* Add A'*e to y * Add A'*e to y
* y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2] * y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2]
*/ */
GTSAM_EXPORT void transposeMultiplyAdd(const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y); void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const;
};
} // namespace gtsam } // namespace gtsam
#endif

40
gtsam/linear/SubgraphSolver.cpp
View File

@ -9,8 +9,6 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
#if 0
#include <gtsam/linear/Errors.h> #include <gtsam/linear/Errors.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
@ -29,60 +27,64 @@ using namespace std;
namespace gtsam { namespace gtsam {
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters) SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters, const Ordering& ordering)
: parameters_(parameters) : parameters_(parameters), ordering_(ordering)
{ {
initialize(gfg); initialize(gfg);
} }
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters &parameters) SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters &parameters, const Ordering& ordering)
: parameters_(parameters) : parameters_(parameters), ordering_(ordering)
{ {
initialize(*jfg); initialize(*jfg);
} }
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters) SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering)
: parameters_(parameters) { : parameters_(parameters), ordering_(ordering) {
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(Ab1)->eliminate(&EliminateQR); GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, EliminateQR);
initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2)); initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
} }
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1,
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters) const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters, const Ordering& ordering)
: parameters_(parameters) { : parameters_(parameters), ordering_(ordering) {
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR); GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR);
initialize(Rc1, Ab2); initialize(Rc1, Ab2);
} }
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2,
const Parameters &parameters) : parameters_(parameters) const Parameters &parameters, const Ordering& ordering) : parameters_(parameters), ordering_(ordering)
{ {
initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2)); initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
} }
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters) : parameters_(parameters) const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters, const Ordering& ordering) :
parameters_(parameters), ordering_(ordering)
{ {
initialize(Rc1, Ab2); initialize(Rc1, Ab2);
} }
/**************************************************************************************************/
VectorValues SubgraphSolver::optimize() { VectorValues SubgraphSolver::optimize() {
VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>(*pc_, pc_->zero(), parameters_); VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>(*pc_, pc_->zero(), parameters_);
return pc_->x(ybar); return pc_->x(ybar);
} }
/**************************************************************************************************/
VectorValues SubgraphSolver::optimize(const VectorValues &initial) { VectorValues SubgraphSolver::optimize(const VectorValues &initial) {
// the initial is ignored in this case ... // the initial is ignored in this case ...
return optimize(); return optimize();
} }
/**************************************************************************************************/
void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) void SubgraphSolver::initialize(const GaussianFactorGraph &jfg)
{ {
GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>(), GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>(),
@ -92,17 +94,19 @@ void SubgraphSolver::initialize(const GaussianFactorGraph &jfg)
if (parameters_.verbosity()) if (parameters_.verbosity())
cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl; cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl;
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR); GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR);
VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(Rc1->optimize());
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar); pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
} }
/**************************************************************************************************/
void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2) void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2)
{ {
VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1))); VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(Rc1->optimize());
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar); pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
} }
/**************************************************************************************************/
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
@ -140,5 +144,3 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
} }
} // \namespace gtsam } // \namespace gtsam
#endif

28
gtsam/linear/SubgraphSolver.h
View File

@ -14,9 +14,16 @@
#include <gtsam/linear/ConjugateGradientSolver.h> #include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h> #include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/inference/Ordering.h>
#include <boost/tuple/tuple.hpp>
namespace gtsam { namespace gtsam {
// Forward declarations
class GaussianFactorGraph;
class GaussianBayesNet;
class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters { class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters {
public: public:
typedef ConjugateGradientParameters Base; typedef ConjugateGradientParameters Base;
@ -24,8 +31,6 @@ public:
virtual void print() const { Base::print(); } virtual void print() const { Base::print(); }
}; };
#if 0
/** /**
* This class implements the SPCG solver presented in Dellaert et al in IROS'10. * This class implements the SPCG solver presented in Dellaert et al in IROS'10.
* *
@ -55,20 +60,21 @@ public:
protected: protected:
Parameters parameters_; Parameters parameters_;
Ordering ordering_;
SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object
public: public:
/* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */
SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters); SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters, const Ordering& ordering);
SubgraphSolver(const GaussianFactorGraph::shared_ptr &A, const Parameters &parameters); SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A, const Parameters &parameters, const Ordering& ordering);
/* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */ /* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */
SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters); SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering);
SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters); SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering);
/* The same as above, but the A1 is solved before */ /* The same as above, but the A1 is solved before */
SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters &parameters); SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering);
SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters); SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering);
virtual ~SubgraphSolver() {} virtual ~SubgraphSolver() {}
virtual VectorValues optimize () ; virtual VectorValues optimize () ;
@ -77,12 +83,10 @@ public:
protected: protected:
void initialize(const GaussianFactorGraph &jfg); void initialize(const GaussianFactorGraph &jfg);
void initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2); void initialize(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2);
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> boost::tuple<boost::shared_ptr<GaussianFactorGraph>, boost::shared_ptr<GaussianFactorGraph> >
splitGraph(const GaussianFactorGraph &gfg) ; splitGraph(const GaussianFactorGraph &gfg) ;
}; };
#endif
} // namespace gtsam } // namespace gtsam

12
gtsam/linear/iterative-inl.h
View File

@ -45,11 +45,11 @@ namespace gtsam {
// Start with g0 = A'*(A*x0-b), d0 = - g0 // Start with g0 = A'*(A*x0-b), d0 = - g0
// i.e., first step is in direction of negative gradient // i.e., first step is in direction of negative gradient
g = gradient(Ab,x); g = Ab.gradient(x);
d = g; // instead of negating gradient, alpha will be negated d = g; // instead of negating gradient, alpha will be negated
// init gamma and calculate threshold // init gamma and calculate threshold
gamma = dot(g,g) ; gamma = dot(g,g);
threshold = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma); threshold = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma);
// Allocate and calculate A*d for first iteration // Allocate and calculate A*d for first iteration
@ -86,9 +86,9 @@ namespace gtsam {
double alpha = takeOptimalStep(x); double alpha = takeOptimalStep(x);
// update gradient (or re-calculate at reset time) // update gradient (or re-calculate at reset time)
if (k % parameters_.reset() == 0) g = gradient(Ab,x); if (k % parameters_.reset() == 0) g = Ab.gradient(x);
// axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad) // axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad)
else transposeMultiplyAdd(Ab, alpha, Ad, g); else Ab.transposeMultiplyAdd(alpha, Ad, g);
// check for convergence // check for convergence
double new_gamma = dot(g, g); double new_gamma = dot(g, g);
@ -105,14 +105,14 @@ namespace gtsam {
else { else {
double beta = new_gamma / gamma; double beta = new_gamma / gamma;
// d = g + d*beta; // d = g + d*beta;
scal(beta, d); d *= beta;
axpy(1.0, g, d); axpy(1.0, g, d);
} }
gamma = new_gamma; gamma = new_gamma;
// In-place recalculation Ad <- A*d to avoid re-allocating Ad // In-place recalculation Ad <- A*d to avoid re-allocating Ad
multiplyInPlace(Ab, d, Ad); Ab.multiplyInPlace(d, Ad);
return false; return false;
} }

14
gtsam/linear/iterative.cpp
View File

@ -19,7 +19,7 @@
#include <gtsam/linear/iterative-inl.h> #include <gtsam/linear/iterative-inl.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h> #include <gtsam/linear/IterativeSolver.h>
#include <iostream> #include <iostream>
@ -61,15 +61,15 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValuesOrdered steepestDescent(const GaussianFactorGraphOrdered& fg, VectorValues steepestDescent(const GaussianFactorGraph& fg,
const VectorValuesOrdered& x, const ConjugateGradientParameters & parameters) { const VectorValues& x, const ConjugateGradientParameters & parameters) {
return conjugateGradients<GaussianFactorGraphOrdered, VectorValuesOrdered, Errors>( return conjugateGradients<GaussianFactorGraph, VectorValues, Errors>(
fg, x, parameters, true); fg, x, parameters, true);
} }
VectorValuesOrdered conjugateGradientDescent(const GaussianFactorGraphOrdered& fg, VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg,
const VectorValuesOrdered& x, const ConjugateGradientParameters & parameters) { const VectorValues& x, const ConjugateGradientParameters & parameters) {
return conjugateGradients<GaussianFactorGraphOrdered, VectorValuesOrdered, Errors>( return conjugateGradients<GaussianFactorGraph, VectorValues, Errors>(
fg, x, parameters); fg, x, parameters);
} }

32
gtsam/linear/iterative.h
View File

@ -19,7 +19,7 @@
#pragma once #pragma once
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/linear/VectorValuesOrdered.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/ConjugateGradientSolver.h> #include <gtsam/linear/ConjugateGradientSolver.h>
namespace gtsam { namespace gtsam {
@ -68,27 +68,27 @@ namespace gtsam {
* Print with optional string * Print with optional string
*/ */
void print (const std::string& s = "System") const; void print (const std::string& s = "System") const;
};
/** gradient of objective function 0.5*|Ax-b_|^2 at x = A_'*(Ax-b_) */ /** gradient of objective function 0.5*|Ax-b_|^2 at x = A_'*(Ax-b_) */
inline Vector gradient(const System& system, const Vector& x) { Vector gradient(const Vector& x) const {
return system.A() ^ (system.A() * x - system.b()); return A() ^ (A() * x - b());
} }
/** Apply operator A */ /** Apply operator A */
inline Vector operator*(const System& system, const Vector& x) { Vector operator*(const Vector& x) const {
return system.A() * x; return A() * x;
} }
/** Apply operator A in place */ /** Apply operator A in place */
inline void multiplyInPlace(const System& system, const Vector& x, Vector& e) { void multiplyInPlace(const Vector& x, Vector& e) const {
e = system.A() * x; e = A() * x;
} }
/** x += alpha* A'*e */ /** x += alpha* A'*e */
inline void transposeMultiplyAdd(const System& system, double alpha, const Vector& e, Vector& x) { void transposeMultiplyAdd(double alpha, const Vector& e, Vector& x) const {
transposeMultiplyAdd(alpha,system.A(),e,x); gtsam::transposeMultiplyAdd(alpha, A(), e, x);
} }
};
/** /**
* Method of steepest gradients, System version * Method of steepest gradients, System version
@ -129,17 +129,17 @@ namespace gtsam {
/** /**
* Method of steepest gradients, Gaussian Factor Graph version * Method of steepest gradients, Gaussian Factor Graph version
*/ */
GTSAM_EXPORT VectorValuesOrdered steepestDescent( GTSAM_EXPORT VectorValues steepestDescent(
const GaussianFactorGraphOrdered& fg, const GaussianFactorGraph& fg,
const VectorValuesOrdered& x, const VectorValues& x,
const ConjugateGradientParameters & parameters); const ConjugateGradientParameters & parameters);
/** /**
* Method of conjugate gradients (CG), Gaussian Factor Graph version * Method of conjugate gradients (CG), Gaussian Factor Graph version
*/ */
GTSAM_EXPORT VectorValuesOrdered conjugateGradientDescent( GTSAM_EXPORT VectorValues conjugateGradientDescent(
const GaussianFactorGraphOrdered& fg, const GaussianFactorGraph& fg,
const VectorValuesOrdered& x, const VectorValues& x,
const ConjugateGradientParameters & parameters); const ConjugateGradientParameters & parameters);

18
gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp
View File

@ -63,16 +63,14 @@ VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const Succ
delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize(); delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize();
} }
else if ( params.isCG() ) { else if ( params.isCG() ) {
throw std::runtime_error("Not implemented"); if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ...");
if ( boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
//if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ..."); SubgraphSolver solver(gfg, dynamic_cast<const SubgraphSolverParameters&>(*params.iterativeParams), *params.ordering);
//if ( boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) { delta = solver.optimize();
// SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams)); }
// delta = solver.optimize(); else {
//} throw std::runtime_error("solveGaussianFactorGraph: special cg parameter type is not handled in LM solver ...");
//else { }
// throw std::runtime_error("solveGaussianFactorGraph: special cg parameter type is not handled in LM solver ...");
//}
} }
else { else {
throw std::runtime_error("solveGaussianFactorGraph: Optimization parameter is invalid"); throw std::runtime_error("solveGaussianFactorGraph: Optimization parameter is invalid");