Started to fix errors resulting from removing Ordered classes
Richard Roberts
parent
4db55a3a6a
commit
5eb7613f5c
|
|
@ -21,11 +21,7 @@
|
|||
|
||||
#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/BayesTree.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
|
|
|
|||
|
|
@ -18,7 +18,9 @@
|
|||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/range/adaptor/map.hpp>
|
||||
#include <gtsam/linear/Errors.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -28,11 +30,9 @@ namespace gtsam {
|
|||
Errors::Errors(){}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Errors::Errors(const VectorValuesOrdered &V) {
|
||||
this->resize(V.size()) ;
|
||||
int i = 0 ;
|
||||
BOOST_FOREACH( Vector &e, *this) {
|
||||
e = V[i++];
|
||||
Errors::Errors(const VectorValues& V) {
|
||||
BOOST_FOREACH(const Vector& e, V | boost::adaptors::map_values) {
|
||||
push_back(e);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -19,19 +19,25 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/VectorValuesOrdered.h>
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
||||
// Forward declarations
|
||||
class VectorValues;
|
||||
|
||||
/** vector of errors */
|
||||
class Errors : public std::list<Vector> {
|
||||
class Errors : public FastList<Vector> {
|
||||
|
||||
public:
|
||||
|
||||
GTSAM_EXPORT Errors() ;
|
||||
|
||||
/** break V into pieces according to its start indices */
|
||||
GTSAM_EXPORT Errors(const VectorValuesOrdered &V) ;
|
||||
/** break V into pieces according to its start indices */
|
||||
GTSAM_EXPORT Errors(const VectorValues&V);
|
||||
|
||||
/** print */
|
||||
GTSAM_EXPORT void print(const std::string& s = "Errors") const;
|
||||
|
|
@ -51,13 +57,13 @@ namespace gtsam {
|
|||
}; // Errors
|
||||
|
||||
/**
|
||||
* dot product
|
||||
*/
|
||||
* dot product
|
||||
*/
|
||||
GTSAM_EXPORT double dot(const Errors& a, const Errors& b);
|
||||
|
||||
/**
|
||||
* BLAS level 2 style
|
||||
*/
|
||||
* BLAS level 2 style
|
||||
*/
|
||||
template <>
|
||||
GTSAM_EXPORT void axpy<Errors,Errors>(double alpha, const Errors& x, Errors& y);
|
||||
|
||||
|
|
|
|||
|
|
@ -55,8 +55,6 @@ namespace gtsam {
|
|||
template<class DERIVEDCONDITIONAL>
|
||||
GaussianBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {}
|
||||
|
||||
/**
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Testable
|
||||
|
|
|
|||
|
|
@ -23,31 +23,27 @@ using namespace std;
|
|||
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 ";
|
||||
for(const_iterator it = beginFrontals(); it != endFrontals(); ++it)
|
||||
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
|
||||
cout << endl;
|
||||
gtsam::print(Matrix(get_R()),"R");
|
||||
gtsam::print(Vector(get_d()),"d");
|
||||
gtsam::print(sigmas_,"sigmas");
|
||||
gtsam::print(Matrix(get_R()), "R: ");
|
||||
gtsam::print(Vector(get_d()), "d: ");
|
||||
if(model_)
|
||||
model_->print("Noise model: ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector GaussianDensity::mean() const {
|
||||
// Solve for mean
|
||||
VectorValuesOrdered x;
|
||||
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];
|
||||
VectorValues soln = this->solve(VectorValues());
|
||||
return soln[firstFrontalKey()];
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix GaussianDensity::covariance() const {
|
||||
return inverse(information());
|
||||
return information().inverse();
|
||||
}
|
||||
|
||||
} // gtsam
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@
|
|||
// \callgraph
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/GaussianConditionalOrdered.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -30,7 +30,7 @@ namespace gtsam {
|
|||
* 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$
|
||||
*/
|
||||
class GTSAM_EXPORT GaussianDensity: public GaussianConditionalOrdered {
|
||||
class GTSAM_EXPORT GaussianDensity : public GaussianConditional {
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -38,24 +38,25 @@ namespace gtsam {
|
|||
|
||||
/// default constructor needed for serialization
|
||||
GaussianDensity() :
|
||||
GaussianConditionalOrdered() {
|
||||
GaussianConditional() {
|
||||
}
|
||||
|
||||
/// Copy constructor from GaussianConditional
|
||||
GaussianDensity(const GaussianConditionalOrdered& conditional) :
|
||||
GaussianConditionalOrdered(conditional) {
|
||||
assert(conditional.nrParents() == 0);
|
||||
GaussianDensity(const GaussianConditional& conditional) :
|
||||
GaussianConditional(conditional) {
|
||||
if(conditional.nrParents() != 0)
|
||||
throw std::invalid_argument("GaussianDensity can only be created from a conditional with no parents");
|
||||
}
|
||||
|
||||
/// constructor using d, R
|
||||
GaussianDensity(Index key, const Vector& d, const Matrix& R,
|
||||
const Vector& sigmas) :
|
||||
GaussianConditionalOrdered(key, d, R, sigmas) {
|
||||
const SharedDiagonal& noiseModel) :
|
||||
GaussianConditional(key, d, R, noiseModel) {
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const std::string& = "GaussianDensity",
|
||||
const IndexFormatter& formatter =DefaultIndexFormatter) const;
|
||||
const KeyFormatter& formatter = DefaultKeyFormatter) const;
|
||||
|
||||
/// Mean \f$ \mu = R^{-1} d \f$
|
||||
Vector mean() const;
|
||||
|
|
|
|||
|
|
@ -11,12 +11,16 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
|
||||
#include <gtsam/linear/VectorValuesOrdered.h>
|
||||
#include <gtsam/global_includes.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class VectorValues;
|
||||
class GaussianFactorGraph;
|
||||
|
||||
/**
|
||||
* parameters for iterative linear solvers
|
||||
*/
|
||||
|
|
@ -67,13 +71,13 @@ namespace gtsam {
|
|||
virtual ~IterativeSolver() {}
|
||||
|
||||
/* interface to the nonlinear optimizer */
|
||||
virtual VectorValuesOrdered optimize () = 0;
|
||||
virtual VectorValues optimize () = 0;
|
||||
|
||||
/* 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 */
|
||||
virtual void replaceFactors(const GaussianFactorGraphOrdered::shared_ptr &factorGraph, const double lambda) {}
|
||||
virtual void replaceFactors(const boost::shared_ptr<GaussianFactorGraph> &factorGraph, const double lambda) {}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -15,8 +15,10 @@
|
|||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#if 0
|
||||
|
||||
#include <gtsam/linear/SubgraphPreconditioner.h>
|
||||
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -24,12 +26,12 @@ using namespace std;
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
static GaussianFactorGraphOrdered::shared_ptr convertToJacobianFactors(const GaussianFactorGraphOrdered &gfg) {
|
||||
GaussianFactorGraphOrdered::shared_ptr result(new GaussianFactorGraphOrdered());
|
||||
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr &gf, gfg) {
|
||||
JacobianFactorOrdered::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactorOrdered>(gf);
|
||||
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
|
||||
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
|
||||
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
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);
|
||||
}
|
||||
|
|
@ -44,34 +46,34 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// 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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double error(const SubgraphPreconditioner& sp, const VectorValuesOrdered& y) {
|
||||
double error(const SubgraphPreconditioner& sp, const VectorValues& y) {
|
||||
Errors e(y);
|
||||
VectorValuesOrdered x = sp.x(y);
|
||||
VectorValues x = sp.x(y);
|
||||
Errors e2 = gaussianErrors(*sp.Ab2(),x);
|
||||
return 0.5 * (dot(e, e) + dot(e2,e2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
|
||||
VectorValuesOrdered gradient(const SubgraphPreconditioner& sp, const VectorValuesOrdered& y) {
|
||||
VectorValuesOrdered x = gtsam::backSubstitute(*sp.Rc1(), y); /* inv(R1)*y */
|
||||
VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y) {
|
||||
VectorValues x = gtsam::backSubstitute(*sp.Rc1(), y); /* inv(R1)*y */
|
||||
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) */
|
||||
return y + gtsam::backSubstituteTranspose(*sp.Rc1(), v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// 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);
|
||||
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 */
|
||||
e.splice(e.end(), e2);
|
||||
return e;
|
||||
|
|
@ -79,7 +81,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// 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();
|
||||
for ( Index i = 0 ; i < y.size() ; ++i, ++ei ) {
|
||||
|
|
@ -87,16 +89,16 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
// 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
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// 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();
|
||||
VectorValuesOrdered y = sp.zero();
|
||||
VectorValues y = sp.zero();
|
||||
for ( Index i = 0 ; i < y.size() ; ++i, ++it )
|
||||
y[i] = *it ;
|
||||
sp.transposeMultiplyAdd2(1.0,it,e.end(),y);
|
||||
|
|
@ -106,7 +108,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
// y += alpha*A'*e
|
||||
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();
|
||||
for ( Index i = 0 ; i < y.size() ; ++i, ++it ) {
|
||||
|
|
@ -119,14 +121,14 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
// y += alpha*inv(R1')*A2'*e2
|
||||
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
|
||||
// TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
|
||||
Errors e2;
|
||||
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
|
||||
axpy(alpha, gtsam::backSubstituteTranspose(*Rc1_, x), y); // y += alpha*inv(R1')*x
|
||||
}
|
||||
|
|
@ -137,3 +139,5 @@ namespace gtsam {
|
|||
Ab2_->print();
|
||||
}
|
||||
} // nsamespace gtsam
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -17,12 +17,21 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/JacobianFactorOrdered.h>
|
||||
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
|
||||
#include <gtsam/linear/GaussianBayesNetOrdered.h>
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/Errors.h>
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#if 0
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class GaussianBayesNet;
|
||||
class GaussianFactorGraph;
|
||||
class VectorValues;
|
||||
|
||||
/**
|
||||
* 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
|
||||
|
|
@ -34,9 +43,9 @@ namespace gtsam {
|
|||
|
||||
public:
|
||||
typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
|
||||
typedef boost::shared_ptr<const GaussianBayesNetOrdered> sharedBayesNet;
|
||||
typedef boost::shared_ptr<const GaussianFactorGraphOrdered> sharedFG;
|
||||
typedef boost::shared_ptr<const VectorValuesOrdered> sharedValues;
|
||||
typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet;
|
||||
typedef boost::shared_ptr<const GaussianFactorGraph> sharedFG;
|
||||
typedef boost::shared_ptr<const VectorValues> sharedValues;
|
||||
typedef boost::shared_ptr<const Errors> sharedErrors;
|
||||
|
||||
private:
|
||||
|
|
@ -73,11 +82,11 @@ namespace gtsam {
|
|||
// SubgraphPreconditioner add_priors(double sigma) const;
|
||||
|
||||
/* 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 */
|
||||
VectorValuesOrdered zero() const {
|
||||
VectorValuesOrdered V(VectorValuesOrdered::Zero(*xbar_));
|
||||
VectorValues zero() const {
|
||||
VectorValues V(VectorValues::Zero(*xbar_));
|
||||
return V ;
|
||||
}
|
||||
|
||||
|
|
@ -87,31 +96,33 @@ namespace gtsam {
|
|||
* Takes a range indicating e2 !!!!
|
||||
*/
|
||||
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 */
|
||||
void print(const std::string& s = "SubgraphPreconditioner") const;
|
||||
};
|
||||
|
||||
/* 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) */
|
||||
GTSAM_EXPORT VectorValuesOrdered gradient(const SubgraphPreconditioner& sp, const VectorValuesOrdered& y);
|
||||
GTSAM_EXPORT VectorValues gradient(const SubgraphPreconditioner& sp, const VectorValues& y);
|
||||
|
||||
/** 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 */
|
||||
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' */
|
||||
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
|
||||
* 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
|
||||
|
||||
#endif
|
||||
|
|
@ -9,16 +9,16 @@
|
|||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
#if 0
|
||||
|
||||
#include <gtsam/linear/Errors.h>
|
||||
#include <gtsam/linear/GaussianConditionalOrdered.h>
|
||||
#include <gtsam/linear/GaussianBayesNetOrdered.h>
|
||||
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.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/VectorValuesOrdered.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/inference/graph-inl.h>
|
||||
#include <gtsam/inference/EliminationTreeOrdered.h>
|
||||
#include <gtsam/base/DSFVector.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
|
@ -29,92 +29,92 @@ using namespace std;
|
|||
namespace gtsam {
|
||||
|
||||
/**************************************************************************************************/
|
||||
SubgraphSolver::SubgraphSolver(const GaussianFactorGraphOrdered &gfg, const Parameters ¶meters)
|
||||
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters)
|
||||
: parameters_(parameters)
|
||||
{
|
||||
initialize(gfg);
|
||||
}
|
||||
|
||||
/**************************************************************************************************/
|
||||
SubgraphSolver::SubgraphSolver(const GaussianFactorGraphOrdered::shared_ptr &jfg, const Parameters ¶meters)
|
||||
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters ¶meters)
|
||||
: parameters_(parameters)
|
||||
{
|
||||
initialize(*jfg);
|
||||
}
|
||||
|
||||
/**************************************************************************************************/
|
||||
SubgraphSolver::SubgraphSolver(const GaussianFactorGraphOrdered &Ab1, const GaussianFactorGraphOrdered &Ab2, const Parameters ¶meters)
|
||||
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters)
|
||||
: parameters_(parameters) {
|
||||
|
||||
GaussianBayesNetOrdered::shared_ptr Rc1 = EliminationTreeOrdered<GaussianFactorOrdered>::Create(Ab1)->eliminate(&EliminateQROrdered);
|
||||
initialize(Rc1, boost::make_shared<GaussianFactorGraphOrdered>(Ab2));
|
||||
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(Ab1)->eliminate(&EliminateQR);
|
||||
initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
|
||||
}
|
||||
|
||||
/**************************************************************************************************/
|
||||
SubgraphSolver::SubgraphSolver(const GaussianFactorGraphOrdered::shared_ptr &Ab1,
|
||||
const GaussianFactorGraphOrdered::shared_ptr &Ab2, const Parameters ¶meters)
|
||||
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1,
|
||||
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters)
|
||||
: 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);
|
||||
}
|
||||
|
||||
/**************************************************************************************************/
|
||||
SubgraphSolver::SubgraphSolver(const GaussianBayesNetOrdered::shared_ptr &Rc1, const GaussianFactorGraphOrdered &Ab2,
|
||||
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2,
|
||||
const Parameters ¶meters) : parameters_(parameters)
|
||||
{
|
||||
initialize(Rc1, boost::make_shared<GaussianFactorGraphOrdered>(Ab2));
|
||||
initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
|
||||
}
|
||||
|
||||
/**************************************************************************************************/
|
||||
SubgraphSolver::SubgraphSolver(const GaussianBayesNetOrdered::shared_ptr &Rc1,
|
||||
const GaussianFactorGraphOrdered::shared_ptr &Ab2, const Parameters ¶meters) : parameters_(parameters)
|
||||
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
|
||||
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) : parameters_(parameters)
|
||||
{
|
||||
initialize(Rc1, Ab2);
|
||||
}
|
||||
|
||||
VectorValuesOrdered SubgraphSolver::optimize() {
|
||||
VectorValuesOrdered ybar = conjugateGradients<SubgraphPreconditioner, VectorValuesOrdered, Errors>(*pc_, pc_->zero(), parameters_);
|
||||
VectorValues SubgraphSolver::optimize() {
|
||||
VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>(*pc_, pc_->zero(), parameters_);
|
||||
return pc_->x(ybar);
|
||||
}
|
||||
|
||||
VectorValuesOrdered SubgraphSolver::optimize(const VectorValuesOrdered &initial) {
|
||||
VectorValues SubgraphSolver::optimize(const VectorValues &initial) {
|
||||
// the initial is ignored in this case ...
|
||||
return optimize();
|
||||
}
|
||||
|
||||
void SubgraphSolver::initialize(const GaussianFactorGraphOrdered &jfg)
|
||||
void SubgraphSolver::initialize(const GaussianFactorGraph &jfg)
|
||||
{
|
||||
GaussianFactorGraphOrdered::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraphOrdered>(),
|
||||
Ab2 = boost::make_shared<GaussianFactorGraphOrdered>();
|
||||
GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>(),
|
||||
Ab2 = boost::make_shared<GaussianFactorGraph>();
|
||||
|
||||
boost::tie(Ab1, Ab2) = splitGraph(jfg) ;
|
||||
if (parameters_.verbosity())
|
||||
cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl;
|
||||
|
||||
GaussianBayesNetOrdered::shared_ptr Rc1 = EliminationTreeOrdered<GaussianFactorOrdered>::Create(*Ab1)->eliminate(&EliminateQROrdered);
|
||||
VectorValuesOrdered::shared_ptr xbar(new VectorValuesOrdered(gtsam::optimize(*Rc1)));
|
||||
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
|
||||
VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
|
||||
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);
|
||||
}
|
||||
|
||||
boost::tuple<GaussianFactorGraphOrdered::shared_ptr, GaussianFactorGraphOrdered::shared_ptr>
|
||||
SubgraphSolver::splitGraph(const GaussianFactorGraphOrdered &jfg) {
|
||||
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
|
||||
SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
|
||||
|
||||
const VariableIndexOrdered index(jfg);
|
||||
const VariableIndex index(jfg);
|
||||
const size_t n = index.size();
|
||||
DSFVector D(n);
|
||||
|
||||
GaussianFactorGraphOrdered::shared_ptr At(new GaussianFactorGraphOrdered());
|
||||
GaussianFactorGraphOrdered::shared_ptr Ac( new GaussianFactorGraphOrdered());
|
||||
GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
|
||||
GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph());
|
||||
|
||||
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 ) {
|
||||
throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
|
||||
|
|
@ -140,3 +140,5 @@ SubgraphSolver::splitGraph(const GaussianFactorGraphOrdered &jfg) {
|
|||
}
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -11,11 +11,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <gtsam/linear/ConjugateGradientSolver.h>
|
||||
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
|
||||
#include <gtsam/linear/GaussianFactorGraphOrdered.h>
|
||||
#include <gtsam/linear/SubgraphPreconditioner.h>
|
||||
#include <gtsam/linear/GaussianBayesNetOrdered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -26,6 +24,8 @@ public:
|
|||
virtual void print() const { Base::print(); }
|
||||
};
|
||||
|
||||
#if 0
|
||||
|
||||
/**
|
||||
* This class implements the SPCG solver presented in Dellaert et al in IROS'10.
|
||||
*
|
||||
|
|
@ -59,30 +59,30 @@ protected:
|
|||
|
||||
public:
|
||||
/* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */
|
||||
SubgraphSolver(const GaussianFactorGraphOrdered &A, const Parameters ¶meters);
|
||||
SubgraphSolver(const GaussianFactorGraphOrdered::shared_ptr &A, const Parameters ¶meters);
|
||||
SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters);
|
||||
SubgraphSolver(const GaussianFactorGraph::shared_ptr &A, const Parameters ¶meters);
|
||||
|
||||
/* 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 ¶meters);
|
||||
SubgraphSolver(const GaussianFactorGraphOrdered::shared_ptr &Ab1, const GaussianFactorGraphOrdered::shared_ptr &Ab2, const Parameters ¶meters);
|
||||
SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters);
|
||||
SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters);
|
||||
|
||||
/* The same as above, but the A1 is solved before */
|
||||
SubgraphSolver(const GaussianBayesNetOrdered::shared_ptr &Rc1, const GaussianFactorGraphOrdered &Ab2, const Parameters ¶meters);
|
||||
SubgraphSolver(const GaussianBayesNetOrdered::shared_ptr &Rc1, const GaussianFactorGraphOrdered::shared_ptr &Ab2, const Parameters ¶meters);
|
||||
SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters);
|
||||
SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters);
|
||||
|
||||
virtual ~SubgraphSolver() {}
|
||||
virtual VectorValuesOrdered optimize () ;
|
||||
virtual VectorValuesOrdered optimize (const VectorValuesOrdered &initial) ;
|
||||
virtual VectorValues optimize () ;
|
||||
virtual VectorValues optimize (const VectorValues &initial) ;
|
||||
|
||||
protected:
|
||||
|
||||
void initialize(const GaussianFactorGraphOrdered &jfg);
|
||||
void initialize(const GaussianBayesNetOrdered::shared_ptr &Rc1, const GaussianFactorGraphOrdered::shared_ptr &Ab2);
|
||||
void initialize(const GaussianFactorGraph &jfg);
|
||||
void initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2);
|
||||
|
||||
boost::tuple<GaussianFactorGraphOrdered::shared_ptr, GaussianFactorGraphOrdered::shared_ptr>
|
||||
splitGraph(const GaussianFactorGraphOrdered &gfg) ;
|
||||
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
|
||||
splitGraph(const GaussianFactorGraph &gfg) ;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
#include <gtsam/linear/SubgraphSolver.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
|
||||
|
|
@ -64,8 +65,7 @@ public:
|
|||
switch (linearSolverType) {
|
||||
case MULTIFRONTAL_CHOLESKY:
|
||||
case SEQUENTIAL_CHOLESKY:
|
||||
throw std::runtime_error("Not implemented");
|
||||
//return EliminatePreferCholeskyOrdered;
|
||||
return EliminatePreferCholesky;
|
||||
|
||||
case MULTIFRONTAL_QR:
|
||||
case SEQUENTIAL_QR:
|
||||
|
|
@ -73,15 +73,13 @@ public:
|
|||
|
||||
default:
|
||||
throw std::runtime_error("Nonlinear optimization parameter \"factorization\" is invalid");
|
||||
return EliminateQR;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
std::string getLinearSolverType() const { return linearSolverTranslator(linearSolverType); }
|
||||
|
||||
void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); }
|
||||
void setIterativeParams(const SubgraphSolverParameters ¶ms);
|
||||
void setIterativeParams(const SubgraphSolverParameters& params);
|
||||
void setOrdering(const Ordering& ordering) { this->ordering = ordering; }
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -20,7 +20,6 @@
|
|||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/linear/GaussianFactorOrdered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue