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

Started to fix errors resulting from removing Ordered classes

release/4.3a0
Richard Roberts 2013-08-02 22:09:49 +00:00
parent 4db55a3a6a
commit 5eb7613f5c
13 changed files with 155 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/inference/FactorGraph-inst.h
View File

@ -21,11 +21,7 @@
#pragma once #pragma once
#include <gtsam/base/FastSet.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/inference/VariableIndexOrdered.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/BayesTree.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>

10
gtsam/linear/Errors.cpp
View File

@ -18,7 +18,9 @@
*/ */
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/range/adaptor/map.hpp>
#include <gtsam/linear/Errors.h> #include <gtsam/linear/Errors.h>
#include <gtsam/linear/VectorValues.h>
using namespace std; using namespace std;
@ -28,11 +30,9 @@ namespace gtsam {
Errors::Errors(){} Errors::Errors(){}
/* ************************************************************************* */ /* ************************************************************************* */
Errors::Errors(const VectorValuesOrdered &V) { Errors::Errors(const VectorValues& V) {
this->resize(V.size()) ; BOOST_FOREACH(const Vector& e, V | boost::adaptors::map_values) {
int i = 0 ; push_back(e);
BOOST_FOREACH( Vector &e, *this) {
e = V[i++];
} }
} }

12
gtsam/linear/Errors.h
View File

@ -19,19 +19,25 @@
#pragma once #pragma once
#include <gtsam/linear/VectorValuesOrdered.h> #include <gtsam/base/FastList.h>
#include <gtsam/base/Vector.h>
#include <string>
namespace gtsam { namespace gtsam {
// Forward declarations
class VectorValues;
/** vector of errors */ /** vector of errors */
class Errors : public std::list<Vector> { class Errors : public FastList<Vector> {
public: public:
GTSAM_EXPORT Errors() ; GTSAM_EXPORT Errors() ;
/** break V into pieces according to its start indices */ /** break V into pieces according to its start indices */
GTSAM_EXPORT Errors(const VectorValuesOrdered &V) ; GTSAM_EXPORT Errors(const VectorValues&V);
/** print */ /** print */
GTSAM_EXPORT void print(const std::string& s = "Errors") const; GTSAM_EXPORT void print(const std::string& s = "Errors") const;

2
gtsam/linear/GaussianBayesNet.h
View File

@ -55,8 +55,6 @@ namespace gtsam {
template<class DERIVEDCONDITIONAL> template<class DERIVEDCONDITIONAL>
GaussianBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {} GaussianBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {}
/**
/// @} /// @}
/// @name Testable /// @name Testable

20
gtsam/linear/GaussianDensity.cpp
View File

@ -23,31 +23,27 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianDensity::print(const string &s, const IndexFormatter& formatter) const void GaussianDensity::print(const string &s, const KeyFormatter& formatter) const
{ {
cout << s << ": density on "; cout << s << ": density on ";
for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) for(const_iterator it = beginFrontals(); it != endFrontals(); ++it)
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
cout << endl; cout << endl;
gtsam::print(Matrix(get_R()),"R"); gtsam::print(Matrix(get_R()), "R: ");
gtsam::print(Vector(get_d()),"d"); gtsam::print(Vector(get_d()), "d: ");
gtsam::print(sigmas_,"sigmas"); if(model_)
model_->print("Noise model: ");
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector GaussianDensity::mean() const { Vector GaussianDensity::mean() const {
// Solve for mean VectorValues soln = this->solve(VectorValues());
VectorValuesOrdered x; return soln[firstFrontalKey()];
Index k = firstFrontalKey();
// a VectorValues that only has a value for k: cannot be printed if k<>0
x.insert(k, Vector(sigmas_.size()));
solveInPlace(x);
return x[k];
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix GaussianDensity::covariance() const { Matrix GaussianDensity::covariance() const {
return inverse(information()); return information().inverse();
} }
} // gtsam } // gtsam

19
gtsam/linear/GaussianDensity.h
View File

@ -19,7 +19,7 @@
// \callgraph // \callgraph
#pragma once #pragma once
#include <gtsam/linear/GaussianConditionalOrdered.h> #include <gtsam/linear/GaussianConditional.h>
namespace gtsam { namespace gtsam {
@ -30,7 +30,7 @@ namespace gtsam {
* The negative log-probability is given by \f$ |Rx - d|^2 \f$ * The negative log-probability is given by \f$ |Rx - d|^2 \f$
* with \f$ \Lambda = \Sigma^{-1} = R^T R \f$ and \f$ \mu = R^{-1} d \f$ * with \f$ \Lambda = \Sigma^{-1} = R^T R \f$ and \f$ \mu = R^{-1} d \f$
*/ */
class GTSAM_EXPORT GaussianDensity: public GaussianConditionalOrdered { class GTSAM_EXPORT GaussianDensity : public GaussianConditional {
public: public:
@ -38,24 +38,25 @@ namespace gtsam {
/// default constructor needed for serialization /// default constructor needed for serialization
GaussianDensity() : GaussianDensity() :
GaussianConditionalOrdered() { GaussianConditional() {
} }
/// Copy constructor from GaussianConditional /// Copy constructor from GaussianConditional
GaussianDensity(const GaussianConditionalOrdered& conditional) : GaussianDensity(const GaussianConditional& conditional) :
GaussianConditionalOrdered(conditional) { GaussianConditional(conditional) {
assert(conditional.nrParents() == 0); if(conditional.nrParents() != 0)
throw std::invalid_argument("GaussianDensity can only be created from a conditional with no parents");
} }
/// constructor using d, R /// constructor using d, R
GaussianDensity(Index key, const Vector& d, const Matrix& R, GaussianDensity(Index key, const Vector& d, const Matrix& R,
const Vector& sigmas) : const SharedDiagonal& noiseModel) :
GaussianConditionalOrdered(key, d, R, sigmas) { GaussianConditional(key, d, R, noiseModel) {
} }
/// print /// print
void print(const std::string& = "GaussianDensity", void print(const std::string& = "GaussianDensity",
const IndexFormatter& formatter =DefaultIndexFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// Mean \f$ \mu = R^{-1} d \f$ /// Mean \f$ \mu = R^{-1} d \f$
Vector mean() const; Vector mean() const;

14
gtsam/linear/IterativeSolver.h
View File

@ -11,12 +11,16 @@
#pragma once #pragma once
#include <gtsam/linear/GaussianFactorGraphOrdered.h> #include <gtsam/global_includes.h>
#include <gtsam/linear/VectorValuesOrdered.h>
#include <string> #include <string>
namespace gtsam { namespace gtsam {
// Forward declarations
class VectorValues;
class GaussianFactorGraph;
/** /**
* parameters for iterative linear solvers * parameters for iterative linear solvers
*/ */
@ -67,13 +71,13 @@ namespace gtsam {
virtual ~IterativeSolver() {} virtual ~IterativeSolver() {}
/* interface to the nonlinear optimizer */ /* interface to the nonlinear optimizer */
virtual VectorValuesOrdered optimize () = 0; virtual VectorValues optimize () = 0;
/* interface to the nonlinear optimizer */ /* interface to the nonlinear optimizer */
virtual VectorValuesOrdered optimize (const VectorValuesOrdered &initial) = 0; virtual VectorValues optimize (const VectorValues &initial) = 0;
/* update interface to the nonlinear optimizer */ /* update interface to the nonlinear optimizer */
virtual void replaceFactors(const GaussianFactorGraphOrdered::shared_ptr &factorGraph, const double lambda) {} virtual void replaceFactors(const boost::shared_ptr<GaussianFactorGraph> &factorGraph, const double lambda) {}
}; };
} }

46
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -15,8 +15,10 @@
* @author: Frank Dellaert * @author: Frank Dellaert
*/ */
#if 0
#include <gtsam/linear/SubgraphPreconditioner.h> #include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
using namespace std; using namespace std;
@ -24,12 +26,12 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
static GaussianFactorGraphOrdered::shared_ptr convertToJacobianFactors(const GaussianFactorGraphOrdered &gfg) { static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
GaussianFactorGraphOrdered::shared_ptr result(new GaussianFactorGraphOrdered()); GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr &gf, gfg) { BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
JacobianFactorOrdered::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactorOrdered>(gf); JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
if( !jf ) { if( !jf ) {
jf = boost::make_shared<JacobianFactorOrdered>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
} }
result->push_back(jf); result->push_back(jf);
} }
@ -44,34 +46,34 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// x = xbar + inv(R1)*y // x = xbar + inv(R1)*y
VectorValuesOrdered SubgraphPreconditioner::x(const VectorValuesOrdered& y) const { VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
return *xbar_ + gtsam::backSubstitute(*Rc1_, y); return *xbar_ + gtsam::backSubstitute(*Rc1_, y);
} }
/* ************************************************************************* */ /* ************************************************************************* */
double error(const SubgraphPreconditioner& sp, const VectorValuesOrdered& y) { double error(const SubgraphPreconditioner& sp, const VectorValues& y) {
Errors e(y); Errors e(y);
VectorValuesOrdered x = sp.x(y); VectorValues x = sp.x(y);
Errors e2 = gaussianErrors(*sp.Ab2(),x); Errors e2 = gaussianErrors(*sp.Ab2(),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),
VectorValuesOrdered gradient(const SubgraphPreconditioner& sp, const VectorValuesOrdered& y) { VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y) {
VectorValuesOrdered x = gtsam::backSubstitute(*sp.Rc1(), y); /* inv(R1)*y */ VectorValues x = gtsam::backSubstitute(*sp.Rc1(), y); /* inv(R1)*y */
Errors e = (*sp.Ab2()*x - *sp.b2bar()); /* (A2*inv(R1)*y-b2bar) */ Errors e = (*sp.Ab2()*x - *sp.b2bar()); /* (A2*inv(R1)*y-b2bar) */
VectorValuesOrdered v = VectorValuesOrdered::Zero(x); VectorValues v = VectorValues::Zero(x);
transposeMultiplyAdd(*sp.Ab2(), 1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ transposeMultiplyAdd(*sp.Ab2(), 1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */
return y + gtsam::backSubstituteTranspose(*sp.Rc1(), v); return y + gtsam::backSubstituteTranspose(*sp.Rc1(), 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 VectorValuesOrdered& y) { Errors operator*(const SubgraphPreconditioner& sp, const VectorValues& y) {
Errors e(y); Errors e(y);
VectorValuesOrdered x = gtsam::backSubstitute(*sp.Rc1(), y); /* x=inv(R1)*y */ VectorValues x = gtsam::backSubstitute(*sp.Rc1(), y); /* x=inv(R1)*y */
Errors e2 = *sp.Ab2() * x; /* A2*x */ Errors e2 = *sp.Ab2() * x; /* A2*x */
e.splice(e.end(), e2); e.splice(e.end(), e2);
return e; return e;
@ -79,7 +81,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// In-place version that overwrites e // In-place version that overwrites e
void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValuesOrdered& y, Errors& e) { void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e) {
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 ) {
@ -87,16 +89,16 @@ namespace gtsam {
} }
// Add A2 contribution // Add A2 contribution
VectorValuesOrdered x = gtsam::backSubstitute(*sp.Rc1(), y); // x=inv(R1)*y VectorValues x = gtsam::backSubstitute(*sp.Rc1(), y); // x=inv(R1)*y
gtsam::multiplyInPlace(*sp.Ab2(), x, ei); // use iterator version gtsam::multiplyInPlace(*sp.Ab2(), 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
VectorValuesOrdered operator^(const SubgraphPreconditioner& sp, const Errors& e) { VectorValues operator^(const SubgraphPreconditioner& sp, const Errors& e) {
Errors::const_iterator it = e.begin(); Errors::const_iterator it = e.begin();
VectorValuesOrdered y = sp.zero(); VectorValues y = sp.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); sp.transposeMultiplyAdd2(1.0,it,e.end(),y);
@ -106,7 +108,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// y += alpha*A'*e // y += alpha*A'*e
void transposeMultiplyAdd void transposeMultiplyAdd
(const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValuesOrdered& y) { (const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y) {
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 ) {
@ -119,14 +121,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// y += alpha*inv(R1')*A2'*e2 // y += alpha*inv(R1')*A2'*e2
void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha, void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
Errors::const_iterator it, Errors::const_iterator end, VectorValuesOrdered& y) const { Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const {
// create e2 with what's left of e // create e2 with what's left of e
// TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ? // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
Errors e2; Errors e2;
while (it != end) e2.push_back(*(it++)); while (it != end) e2.push_back(*(it++));
VectorValuesOrdered x = VectorValuesOrdered::Zero(y); // x = 0 VectorValues x = VectorValues::Zero(y); // x = 0
gtsam::transposeMultiplyAdd(*Ab2_,1.0,e2,x); // x += A2'*e2 gtsam::transposeMultiplyAdd(*Ab2_,1.0,e2,x); // x += A2'*e2
axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x
} }
@ -137,3 +139,5 @@ namespace gtsam {
Ab2_->print(); Ab2_->print();
} }
} // nsamespace gtsam } // nsamespace gtsam
#endif

43
gtsam/linear/SubgraphPreconditioner.h
View File

@ -17,12 +17,21 @@
#pragma once #pragma once
#include <gtsam/linear/JacobianFactorOrdered.h> #include <gtsam/global_includes.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianBayesNetOrdered.h> #include <gtsam/linear/Errors.h>
#include <boost/shared_ptr.hpp>
#if 0
namespace gtsam { namespace gtsam {
// Forward declarations
class GaussianBayesNet;
class GaussianFactorGraph;
class VectorValues;
/** /**
* Subgraph conditioner class, as explained in the RSS 2010 submission. * Subgraph conditioner class, as explained in the RSS 2010 submission.
* Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2 * Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2
@ -34,9 +43,9 @@ namespace gtsam {
public: public:
typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr; typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
typedef boost::shared_ptr<const GaussianBayesNetOrdered> sharedBayesNet; typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet;
typedef boost::shared_ptr<const GaussianFactorGraphOrdered> sharedFG; typedef boost::shared_ptr<const GaussianFactorGraph> sharedFG;
typedef boost::shared_ptr<const VectorValuesOrdered> sharedValues; typedef boost::shared_ptr<const VectorValues> sharedValues;
typedef boost::shared_ptr<const Errors> sharedErrors; typedef boost::shared_ptr<const Errors> sharedErrors;
private: private:
@ -73,11 +82,11 @@ namespace gtsam {
// SubgraphPreconditioner add_priors(double sigma) const; // SubgraphPreconditioner add_priors(double sigma) const;
/* x = xbar + inv(R1)*y */ /* x = xbar + inv(R1)*y */
VectorValuesOrdered x(const VectorValuesOrdered& y) const; VectorValues x(const VectorValues& y) const;
/* A zero VectorValues with the structure of xbar */ /* A zero VectorValues with the structure of xbar */
VectorValuesOrdered zero() const { VectorValues zero() const {
VectorValuesOrdered V(VectorValuesOrdered::Zero(*xbar_)); VectorValues V(VectorValues::Zero(*xbar_));
return V ; return V ;
} }
@ -87,31 +96,33 @@ namespace gtsam {
* Takes a range indicating e2 !!!! * Takes a range indicating e2 !!!!
*/ */
void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin, void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin,
Errors::const_iterator end, VectorValuesOrdered& y) const; Errors::const_iterator end, VectorValues& y) const;
/** print the object */ /** print the object */
void print(const std::string& s = "SubgraphPreconditioner") const; void print(const std::string& s = "SubgraphPreconditioner") const;
}; };
/* error, given y */ /* error, given y */
GTSAM_EXPORT double error(const SubgraphPreconditioner& sp, const VectorValuesOrdered& y); GTSAM_EXPORT double error(const SubgraphPreconditioner& sp, const VectorValues& y);
/** gradient = y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar) */ /** gradient = y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar) */
GTSAM_EXPORT VectorValuesOrdered gradient(const SubgraphPreconditioner& sp, const VectorValuesOrdered& y); GTSAM_EXPORT VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y);
/** Apply operator A */ /** Apply operator A */
GTSAM_EXPORT Errors operator*(const SubgraphPreconditioner& sp, const VectorValuesOrdered& y); GTSAM_EXPORT Errors operator*(const SubgraphPreconditioner& sp, const VectorValues& y);
/** 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 VectorValuesOrdered& y, Errors& e); GTSAM_EXPORT void multiplyInPlace(const SubgraphPreconditioner& sp, const VectorValues& y, Errors& e);
/** Apply operator A' */ /** Apply operator A' */
GTSAM_EXPORT VectorValuesOrdered operator^(const SubgraphPreconditioner& sp, const Errors& e); GTSAM_EXPORT VectorValues operator^(const SubgraphPreconditioner& sp, const Errors& e);
/** /**
* 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, VectorValuesOrdered& y); GTSAM_EXPORT void transposeMultiplyAdd(const SubgraphPreconditioner& sp, double alpha, const Errors& e, VectorValues& y);
} // namespace gtsam } // namespace gtsam
#endif

70
gtsam/linear/SubgraphSolver.cpp
View File

@ -9,16 +9,16 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
#if 0
#include <gtsam/linear/Errors.h> #include <gtsam/linear/Errors.h>
#include <gtsam/linear/GaussianConditionalOrdered.h> #include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNetOrdered.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/linear/iterative-inl.h> #include <gtsam/linear/iterative-inl.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphSolver.h> #include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/VectorValuesOrdered.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph-inl.h> #include <gtsam/inference/graph-inl.h>
#include <gtsam/inference/EliminationTreeOrdered.h>
#include <gtsam/base/DSFVector.h> #include <gtsam/base/DSFVector.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
@ -29,92 +29,92 @@ using namespace std;
namespace gtsam { namespace gtsam {
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraphOrdered &gfg, const Parameters &parameters) SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters)
: parameters_(parameters) : parameters_(parameters)
{ {
initialize(gfg); initialize(gfg);
} }
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraphOrdered::shared_ptr &jfg, const Parameters &parameters) SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters &parameters)
: parameters_(parameters) : parameters_(parameters)
{ {
initialize(*jfg); initialize(*jfg);
} }
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraphOrdered &Ab1, const GaussianFactorGraphOrdered &Ab2, const Parameters &parameters) SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters)
: parameters_(parameters) { : parameters_(parameters) {
GaussianBayesNetOrdered::shared_ptr Rc1 = EliminationTreeOrdered<GaussianFactorOrdered>::Create(Ab1)->eliminate(&EliminateQROrdered); GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(Ab1)->eliminate(&EliminateQR);
initialize(Rc1, boost::make_shared<GaussianFactorGraphOrdered>(Ab2)); initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
} }
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraphOrdered::shared_ptr &Ab1, SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1,
const GaussianFactorGraphOrdered::shared_ptr &Ab2, const Parameters &parameters) const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters)
: parameters_(parameters) { : parameters_(parameters) {
GaussianBayesNetOrdered::shared_ptr Rc1 = EliminationTreeOrdered<GaussianFactorOrdered>::Create(*Ab1)->eliminate(&EliminateQROrdered); GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
initialize(Rc1, Ab2); initialize(Rc1, Ab2);
} }
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianBayesNetOrdered::shared_ptr &Rc1, const GaussianFactorGraphOrdered &Ab2, SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2,
const Parameters &parameters) : parameters_(parameters) const Parameters &parameters) : parameters_(parameters)
{ {
initialize(Rc1, boost::make_shared<GaussianFactorGraphOrdered>(Ab2)); initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
} }
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianBayesNetOrdered::shared_ptr &Rc1, SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
const GaussianFactorGraphOrdered::shared_ptr &Ab2, const Parameters &parameters) : parameters_(parameters) const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters) : parameters_(parameters)
{ {
initialize(Rc1, Ab2); initialize(Rc1, Ab2);
} }
VectorValuesOrdered SubgraphSolver::optimize() { VectorValues SubgraphSolver::optimize() {
VectorValuesOrdered ybar = conjugateGradients<SubgraphPreconditioner, VectorValuesOrdered, Errors>(*pc_, pc_->zero(), parameters_); VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>(*pc_, pc_->zero(), parameters_);
return pc_->x(ybar); return pc_->x(ybar);
} }
VectorValuesOrdered SubgraphSolver::optimize(const VectorValuesOrdered &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 GaussianFactorGraphOrdered &jfg) void SubgraphSolver::initialize(const GaussianFactorGraph &jfg)
{ {
GaussianFactorGraphOrdered::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraphOrdered>(), GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>(),
Ab2 = boost::make_shared<GaussianFactorGraphOrdered>(); Ab2 = boost::make_shared<GaussianFactorGraph>();
boost::tie(Ab1, Ab2) = splitGraph(jfg) ; boost::tie(Ab1, Ab2) = splitGraph(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;
GaussianBayesNetOrdered::shared_ptr Rc1 = EliminationTreeOrdered<GaussianFactorOrdered>::Create(*Ab1)->eliminate(&EliminateQROrdered); GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
VectorValuesOrdered::shared_ptr xbar(new VectorValuesOrdered(gtsam::optimize(*Rc1))); VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar); pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
} }
void SubgraphSolver::initialize(const GaussianBayesNetOrdered::shared_ptr &Rc1, const GaussianFactorGraphOrdered::shared_ptr &Ab2) void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2)
{ {
VectorValuesOrdered::shared_ptr xbar(new VectorValuesOrdered(gtsam::optimize(*Rc1))); VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar); pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
} }
boost::tuple<GaussianFactorGraphOrdered::shared_ptr, GaussianFactorGraphOrdered::shared_ptr> boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
SubgraphSolver::splitGraph(const GaussianFactorGraphOrdered &jfg) { SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
const VariableIndexOrdered index(jfg); const VariableIndex index(jfg);
const size_t n = index.size(); const size_t n = index.size();
DSFVector D(n); DSFVector D(n);
GaussianFactorGraphOrdered::shared_ptr At(new GaussianFactorGraphOrdered()); GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
GaussianFactorGraphOrdered::shared_ptr Ac( new GaussianFactorGraphOrdered()); GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph());
size_t t = 0; size_t t = 0;
BOOST_FOREACH ( const GaussianFactorOrdered::shared_ptr &gf, jfg ) { BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) {
if ( gf->keys().size() > 2 ) { if ( gf->keys().size() > 2 ) {
throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
@ -140,3 +140,5 @@ SubgraphSolver::splitGraph(const GaussianFactorGraphOrdered &jfg) {
} }
} // \namespace gtsam } // \namespace gtsam
#endif

34
gtsam/linear/SubgraphSolver.h
View File

@ -11,11 +11,9 @@
#pragma once #pragma once
#include <gtsam/linear/ConjugateGradientSolver.h> #include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
#include <gtsam/linear/SubgraphPreconditioner.h> #include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianBayesNetOrdered.h>
namespace gtsam { namespace gtsam {
@ -26,6 +24,8 @@ 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.
* *
@ -59,30 +59,30 @@ protected:
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 GaussianFactorGraphOrdered &A, const Parameters &parameters); SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters);
SubgraphSolver(const GaussianFactorGraphOrdered::shared_ptr &A, const Parameters &parameters); SubgraphSolver(const GaussianFactorGraph::shared_ptr &A, const Parameters &parameters);
/* 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 GaussianFactorGraphOrdered &Ab1, const GaussianFactorGraphOrdered &Ab2, const Parameters &parameters); SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters);
SubgraphSolver(const GaussianFactorGraphOrdered::shared_ptr &Ab1, const GaussianFactorGraphOrdered::shared_ptr &Ab2, const Parameters &parameters); SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters);
/* The same as above, but the A1 is solved before */ /* The same as above, but the A1 is solved before */
SubgraphSolver(const GaussianBayesNetOrdered::shared_ptr &Rc1, const GaussianFactorGraphOrdered &Ab2, const Parameters &parameters); SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters &parameters);
SubgraphSolver(const GaussianBayesNetOrdered::shared_ptr &Rc1, const GaussianFactorGraphOrdered::shared_ptr &Ab2, const Parameters &parameters); SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters);
virtual ~SubgraphSolver() {} virtual ~SubgraphSolver() {}
virtual VectorValuesOrdered optimize () ; virtual VectorValues optimize () ;
virtual VectorValuesOrdered optimize (const VectorValuesOrdered &initial) ; virtual VectorValues optimize (const VectorValues &initial) ;
protected: protected:
void initialize(const GaussianFactorGraphOrdered &jfg); void initialize(const GaussianFactorGraph &jfg);
void initialize(const GaussianBayesNetOrdered::shared_ptr &Rc1, const GaussianFactorGraphOrdered::shared_ptr &Ab2); void initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2);
boost::tuple<GaussianFactorGraphOrdered::shared_ptr, GaussianFactorGraphOrdered::shared_ptr> boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
splitGraph(const GaussianFactorGraphOrdered &gfg) ; splitGraph(const GaussianFactorGraph &gfg) ;
}; };
#endif
} // namespace gtsam } // namespace gtsam

8
gtsam/nonlinear/SuccessiveLinearizationOptimizer.h
View File

@ -22,6 +22,7 @@
#include <gtsam/linear/SubgraphSolver.h> #include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
@ -64,8 +65,7 @@ public:
switch (linearSolverType) { switch (linearSolverType) {
case MULTIFRONTAL_CHOLESKY: case MULTIFRONTAL_CHOLESKY:
case SEQUENTIAL_CHOLESKY: case SEQUENTIAL_CHOLESKY:
throw std::runtime_error("Not implemented"); return EliminatePreferCholesky;
//return EliminatePreferCholeskyOrdered;
case MULTIFRONTAL_QR: case MULTIFRONTAL_QR:
case SEQUENTIAL_QR: case SEQUENTIAL_QR:
@ -73,15 +73,13 @@ public:
default: default:
throw std::runtime_error("Nonlinear optimization parameter \"factorization\" is invalid"); throw std::runtime_error("Nonlinear optimization parameter \"factorization\" is invalid");
return EliminateQR;
break;
} }
} }
std::string getLinearSolverType() const { return linearSolverTranslator(linearSolverType); } std::string getLinearSolverType() const { return linearSolverTranslator(linearSolverType); }
void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); } void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); }
void setIterativeParams(const SubgraphSolverParameters &params); void setIterativeParams(const SubgraphSolverParameters& params);
void setOrdering(const Ordering& ordering) { this->ordering = ordering; } void setOrdering(const Ordering& ordering) { this->ordering = ordering; }
private: private:

1
gtsam/slam/BetweenFactor.h
View File

@ -20,7 +20,6 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h> #include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactorOrdered.h>
namespace gtsam { namespace gtsam {