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

Doxygen, header bloat, standard formatting

release/4.3a0
dellaert 2015-02-18 09:30:39 +01:00
parent 1ebb3cf2a9
commit f8f2c2db92
17 changed files with 549 additions and 323 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
.cproject
View File

@ -1522,6 +1522,30 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testSmartProjectionPoseFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSmartProjectionPoseFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRegularImplicitSchurFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testRegularImplicitSchurFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRegularJacobianFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testRegularJacobianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j3</buildArguments> <buildArguments>-j3</buildArguments>

24
gtsam/linear/ConjugateGradientSolver.cpp
View File

@ -1,8 +1,20 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/* /*
* ConjugateGradientSolver.cpp * @file ConjugateGradientSolver.cpp
* * @brief Implementation of Conjugate Gradient solver for a linear system
* Created on: Jun 4, 2014 * @author Yong-Dian Jian
* Author: Yong-Dian Jian * @author Sungtae An
* @date Nov 6, 2014
*/ */
#include <gtsam/linear/ConjugateGradientSolver.h> #include <gtsam/linear/ConjugateGradientSolver.h>
@ -35,7 +47,8 @@ std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value)
} }
/*****************************************************************************/ /*****************************************************************************/
ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(const std::string &src) { ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(
const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s); std::string s = src; boost::algorithm::to_upper(s);
if (s == "GTSAM") return ConjugateGradientParameters::GTSAM; if (s == "GTSAM") return ConjugateGradientParameters::GTSAM;
@ -43,6 +56,7 @@ ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTransla
return ConjugateGradientParameters::GTSAM; return ConjugateGradientParameters::GTSAM;
} }
/*****************************************************************************/
} }

9
gtsam/linear/ConjugateGradientSolver.h
View File

@ -20,7 +20,6 @@
#pragma once #pragma once
#include <gtsam/linear/IterativeSolver.h> #include <gtsam/linear/IterativeSolver.h>
#include <iosfwd>
namespace gtsam { namespace gtsam {
@ -87,9 +86,8 @@ public:
static BLASKernel blasTranslator(const std::string &s) ; static BLASKernel blasTranslator(const std::string &s) ;
}; };
/*********************************************************************************************/
/* /*
* A template of linear preconditioned conjugate gradient method. * A template for the linear preconditioned conjugate gradient method.
* System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y) * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y)
* leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T
* Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book. * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book.
@ -98,8 +96,9 @@ public:
* [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems, * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems,
* 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281. * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281.
*/ */
template <class S, class V> template<class S, class V>
V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters &parameters) { V preconditionedConjugateGradient(const S &system, const V &initial,
const ConjugateGradientParameters &parameters) {
V estimate, residual, direction, q1, q2; V estimate, residual, direction, q1, q2;
estimate = residual = direction = q1 = q2 = initial; estimate = residual = direction = q1 = q2 = initial;

101
gtsam/linear/IterativeSolver.cpp
View File

@ -1,6 +1,17 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file IterativeSolver.cpp * @file IterativeSolver.cpp
* @brief * @brief Some support classes for iterative solvers
* @date Sep 3, 2012 * @date Sep 3, 2012
* @author Yong-Dian Jian * @author Yong-Dian Jian
*/ */
@ -9,18 +20,22 @@
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <boost/foreach.hpp>
#include <iostream> #include <iostream>
#include <string>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
/*****************************************************************************/ /*****************************************************************************/
string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } string IterativeOptimizationParameters::getVerbosity() const {
return verbosityTranslator(verbosity_);
}
/*****************************************************************************/ /*****************************************************************************/
void IterativeOptimizationParameters::setVerbosity(const string &src) { verbosity_ = verbosityTranslator(src); } void IterativeOptimizationParameters::setVerbosity(const string &src) {
verbosity_ = verbosityTranslator(src);
}
/*****************************************************************************/ /*****************************************************************************/
void IterativeOptimizationParameters::print() const { void IterativeOptimizationParameters::print() const {
@ -29,78 +44,82 @@ void IterativeOptimizationParameters::print() const {
/*****************************************************************************/ /*****************************************************************************/
void IterativeOptimizationParameters::print(ostream &os) const { void IterativeOptimizationParameters::print(ostream &os) const {
os << "IterativeOptimizationParameters:" << endl os << "IterativeOptimizationParameters:" << endl << "verbosity: "
<< "verbosity: " << verbosityTranslator(verbosity_) << endl; << verbosityTranslator(verbosity_) << endl;
} }
/*****************************************************************************/ /*****************************************************************************/
ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) {
p.print(os); p.print(os);
return os; return os;
} }
/*****************************************************************************/ /*****************************************************************************/
IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const string &src) { IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(
string s = src; boost::algorithm::to_upper(s); const string &src) {
if (s == "SILENT") return IterativeOptimizationParameters::SILENT; string s = src;
else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY; boost::algorithm::to_upper(s);
else if (s == "ERROR") return IterativeOptimizationParameters::ERROR; if (s == "SILENT")
return IterativeOptimizationParameters::SILENT;
else if (s == "COMPLEXITY")
return IterativeOptimizationParameters::COMPLEXITY;
else if (s == "ERROR")
return IterativeOptimizationParameters::ERROR;
/* default is default */ /* default is default */
else return IterativeOptimizationParameters::SILENT; else
return IterativeOptimizationParameters::SILENT;
} }
/*****************************************************************************/ /*****************************************************************************/
string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { string IterativeOptimizationParameters::verbosityTranslator(
if (verbosity == SILENT) return "SILENT"; IterativeOptimizationParameters::Verbosity verbosity) {
else if (verbosity == COMPLEXITY) return "COMPLEXITY"; if (verbosity == SILENT)
else if (verbosity == ERROR) return "ERROR"; return "SILENT";
else return "UNKNOWN"; else if (verbosity == COMPLEXITY)
return "COMPLEXITY";
else if (verbosity == ERROR)
return "ERROR";
else
return "UNKNOWN";
} }
/*****************************************************************************/ /*****************************************************************************/
VectorValues IterativeSolver::optimize( VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
const GaussianFactorGraph &gfg,
boost::optional<const KeyInfo&> keyInfo, boost::optional<const KeyInfo&> keyInfo,
boost::optional<const std::map<Key, Vector>&> lambda) boost::optional<const std::map<Key, Vector>&> lambda) {
{ return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg),
return optimize( lambda ? *lambda : std::map<Key, Vector>());
gfg,
keyInfo ? *keyInfo : KeyInfo(gfg),
lambda ? *lambda : std::map<Key,Vector>());
} }
/*****************************************************************************/ /*****************************************************************************/
VectorValues IterativeSolver::optimize ( VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) {
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda)
{
return optimize(gfg, keyInfo, lambda, keyInfo.x0()); return optimize(gfg, keyInfo, lambda, keyInfo.x0());
} }
/****************************************************************************/ /****************************************************************************/
KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) :
: ordering_(ordering) { ordering_(ordering) {
initialize(fg); initialize(fg);
} }
/****************************************************************************/ /****************************************************************************/
KeyInfo::KeyInfo(const GaussianFactorGraph &fg) KeyInfo::KeyInfo(const GaussianFactorGraph &fg) :
: ordering_(Ordering::Natural(fg)) { ordering_(Ordering::Natural(fg)) {
initialize(fg); initialize(fg);
} }
/****************************************************************************/ /****************************************************************************/
void KeyInfo::initialize(const GaussianFactorGraph &fg){ void KeyInfo::initialize(const GaussianFactorGraph &fg) {
const map<Key, size_t> colspec = fg.getKeyDimMap(); const map<Key, size_t> colspec = fg.getKeyDimMap();
const size_t n = ordering_.size(); const size_t n = ordering_.size();
size_t start = 0; size_t start = 0;
for ( size_t i = 0 ; i < n ; ++i ) { for (size_t i = 0; i < n; ++i) {
const size_t key = ordering_[i]; const size_t key = ordering_[i];
const size_t dim = colspec.find(key)->second; const size_t dim = colspec.find(key)->second;
insert(make_pair(key, KeyInfoEntry(i, dim, start))); insert(make_pair(key, KeyInfoEntry(i, dim, start)));
start += dim ; start += dim;
} }
numCols_ = start; numCols_ = start;
} }
@ -108,7 +127,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg){
/****************************************************************************/ /****************************************************************************/
vector<size_t> KeyInfo::colSpec() const { vector<size_t> KeyInfo::colSpec() const {
std::vector<size_t> result(size(), 0); std::vector<size_t> result(size(), 0);
BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) {
result[item.second.index()] = item.second.dim(); result[item.second.index()] = item.second.dim();
} }
return result; return result;
@ -117,7 +136,7 @@ vector<size_t> KeyInfo::colSpec() const {
/****************************************************************************/ /****************************************************************************/
VectorValues KeyInfo::x0() const { VectorValues KeyInfo::x0() const {
VectorValues result; VectorValues result;
BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) {
result.insert(item.first, Vector::Zero(item.second.dim())); result.insert(item.first, Vector::Zero(item.second.dim()));
} }
return result; return result;
@ -128,7 +147,5 @@ Vector KeyInfo::x0vector() const {
return Vector::Zero(numCols_); return Vector::Zero(numCols_);
} }
} }

231
gtsam/linear/IterativeSolver.h
View File

@ -9,131 +9,178 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/**
* @file IterativeSolver.h
* @brief Some support classes for iterative solvers
* @date 2010
* @author Yong-Dian Jian
*/
#pragma once #pragma once
#include <gtsam/base/Vector.h>
#include <gtsam/global_includes.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/base/Vector.h>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/optional/optional.hpp> #include <boost/shared_ptr.hpp>
#include <boost/none.hpp> #include <boost/optional.hpp>
#include <iosfwd> #include <iosfwd>
#include <map>
#include <string> #include <string>
#include <vector> #include <map>
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
class KeyInfo; class KeyInfo;
class KeyInfoEntry; class KeyInfoEntry;
class GaussianFactorGraph; class GaussianFactorGraph;
class Values; class Values;
class VectorValues; class VectorValues;
/************************************************************************************/ /**
/** * parameters for iterative linear solvers
* parameters for iterative linear solvers */
*/ class GTSAM_EXPORT IterativeOptimizationParameters {
class GTSAM_EXPORT IterativeOptimizationParameters {
public: public:
typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr; typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr;
enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_; enum Verbosity {
SILENT = 0, COMPLEXITY, ERROR
} verbosity_;
public: public:
IterativeOptimizationParameters(Verbosity v = SILENT) IterativeOptimizationParameters(Verbosity v = SILENT) :
: verbosity_(v) {} verbosity_(v) {
}
virtual ~IterativeOptimizationParameters() {} virtual ~IterativeOptimizationParameters() {
}
/* utility */ /* utility */
inline Verbosity verbosity() const { return verbosity_; } inline Verbosity verbosity() const {
std::string getVerbosity() const; return verbosity_;
void setVerbosity(const std::string &s) ; }
std::string getVerbosity() const;
void setVerbosity(const std::string &s);
/* matlab interface */ /* matlab interface */
void print() const ; void print() const;
/* virtual print function */ /* virtual print function */
virtual void print(std::ostream &os) const ; virtual void print(std::ostream &os) const;
/* for serialization */ /* for serialization */
friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p); friend std::ostream& operator<<(std::ostream &os,
const IterativeOptimizationParameters &p);
static Verbosity verbosityTranslator(const std::string &s); static Verbosity verbosityTranslator(const std::string &s);
static std::string verbosityTranslator(Verbosity v); static std::string verbosityTranslator(Verbosity v);
}; };
/************************************************************************************/ /**
class GTSAM_EXPORT IterativeSolver { * Base class for Iterative Solvers like SubgraphSolver
public: */
typedef boost::shared_ptr<IterativeSolver> shared_ptr; class GTSAM_EXPORT IterativeSolver {
IterativeSolver() {} public:
virtual ~IterativeSolver() {} typedef boost::shared_ptr<IterativeSolver> shared_ptr;
IterativeSolver() {
}
virtual ~IterativeSolver() {
}
/* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
VectorValues optimize ( VectorValues optimize(const GaussianFactorGraph &gfg,
const GaussianFactorGraph &gfg,
boost::optional<const KeyInfo&> = boost::none, boost::optional<const KeyInfo&> = boost::none,
boost::optional<const std::map<Key, Vector>&> lambda = boost::none boost::optional<const std::map<Key, Vector>&> lambda = boost::none);
);
/* interface to the nonlinear optimizer, without initial estimate */ /* interface to the nonlinear optimizer, without initial estimate */
VectorValues optimize ( VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo,
const GaussianFactorGraph &gfg, const std::map<Key, Vector> &lambda);
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda
);
/* interface to the nonlinear optimizer that the subclasses have to implement */ /* interface to the nonlinear optimizer that the subclasses have to implement */
virtual VectorValues optimize ( virtual VectorValues optimize(const GaussianFactorGraph &gfg,
const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
const KeyInfo &keyInfo, const VectorValues &initial) = 0;
const std::map<Key, Vector> &lambda,
const VectorValues &initial
) = 0;
}; };
/************************************************************************************/ /**
/* Handy data structure for iterative solvers * Handy data structure for iterative solvers
* key to (index, dimension, colstart) */ * key to (index, dimension, colstart)
class GTSAM_EXPORT KeyInfoEntry : public boost::tuple<size_t, size_t, size_t> { */
public: class GTSAM_EXPORT KeyInfoEntry: public boost::tuple<size_t, size_t, size_t> {
typedef boost::tuple<Key,size_t,Key> Base;
KeyInfoEntry(){}
KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {}
size_t index() const { return this->get<0>(); }
size_t dim() const { return this->get<1>(); }
size_t colstart() const { return this->get<2>(); }
};
/************************************************************************************/ public:
class GTSAM_EXPORT KeyInfo : public std::map<Key, KeyInfoEntry> {
public:
typedef std::map<Key, KeyInfoEntry> Base;
KeyInfo() : numCols_(0) {}
KeyInfo(const GaussianFactorGraph &fg);
KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering);
std::vector<size_t> colSpec() const ; typedef boost::tuple<Key, size_t, Key> Base;
VectorValues x0() const;
Vector x0vector() const;
inline size_t numCols() const { return numCols_; } KeyInfoEntry() {
inline const Ordering & ordering() const { return ordering_; } }
KeyInfoEntry(size_t idx, size_t d, Key start) :
Base(idx, d, start) {
}
size_t index() const {
return this->get<0>();
}
size_t dim() const {
return this->get<1>();
}
size_t colstart() const {
return this->get<2>();
}
};
protected: /**
* Handy data structure for iterative solvers
*/
class GTSAM_EXPORT KeyInfo: public std::map<Key, KeyInfoEntry> {
void initialize(const GaussianFactorGraph &fg); public:
Ordering ordering_; typedef std::map<Key, KeyInfoEntry> Base;
size_t numCols_;
}; protected:
Ordering ordering_;
size_t numCols_;
} void initialize(const GaussianFactorGraph &fg);
public:
/// Default Constructor
KeyInfo() :
numCols_(0) {
}
/// Construct from Gaussian factor graph, use "Natural" ordering
KeyInfo(const GaussianFactorGraph &fg);
/// Construct from Gaussian factor graph and a given ordering
KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering);
/// Return the total number of columns (scalar variables = sum of dimensions)
inline size_t numCols() const {
return numCols_;
}
/// Return the ordering
inline const Ordering & ordering() const {
return ordering_;
}
/// Return a vector of dimensions ordered by ordering()
std::vector<size_t> colSpec() const;
/// Return VectorValues with zeros, of correct dimension
VectorValues x0() const;
/// Return zero Vector of correct dimension
Vector x0vector() const;
};
} // \ namespace gtsam

79
gtsam/linear/PCGSolver.cpp
View File

@ -1,16 +1,31 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/* /*
* PCGSolver.cpp * @file PCGSolver.cpp
* * @brief Preconditioned Conjugate Gradient Solver for linear systems
* Created on: Feb 14, 2012 * @date Feb 14, 2012
* Author: Yong-Dian Jian * @author Yong-Dian Jian
* Author: Sungtae An * @author Sungtae An
*/ */
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/PCGSolver.h> #include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/Preconditioner.h> #include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <algorithm>
#include <iostream> #include <iostream>
#include <stdexcept> #include <stdexcept>
@ -21,7 +36,7 @@ namespace gtsam {
/*****************************************************************************/ /*****************************************************************************/
void PCGSolverParameters::print(ostream &os) const { void PCGSolverParameters::print(ostream &os) const {
Base::print(os); Base::print(os);
os << "PCGSolverParameters:" << endl; os << "PCGSolverParameters:" << endl;
preconditioner_->print(os); preconditioner_->print(os);
} }
@ -32,30 +47,27 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) {
} }
/*****************************************************************************/ /*****************************************************************************/
VectorValues PCGSolver::optimize ( VectorValues PCGSolver::optimize(const GaussianFactorGraph &gfg,
const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
const KeyInfo &keyInfo, const VectorValues &initial) {
const std::map<Key, Vector> &lambda,
const VectorValues &initial)
{
/* build preconditioner */ /* build preconditioner */
preconditioner_->build(gfg, keyInfo, lambda); preconditioner_->build(gfg, keyInfo, lambda);
/* apply pcg */ /* apply pcg */
const Vector sol = preconditionedConjugateGradient<GaussianFactorGraphSystem, Vector>( GaussianFactorGraphSystem system(gfg, *preconditioner_, keyInfo, lambda);
GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda), Vector x0 = initial.vector(keyInfo.ordering());
initial.vector(keyInfo.ordering()), parameters_); const Vector sol = preconditionedConjugateGradient(system, x0, parameters_);
return buildVectorValues(sol, keyInfo); return buildVectorValues(sol, keyInfo);
} }
/*****************************************************************************/ /*****************************************************************************/
GaussianFactorGraphSystem::GaussianFactorGraphSystem( GaussianFactorGraphSystem::GaussianFactorGraphSystem(
const GaussianFactorGraph &gfg, const GaussianFactorGraph &gfg, const Preconditioner &preconditioner,
const Preconditioner &preconditioner, const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) :
const KeyInfo &keyInfo, gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(
const std::map<Key, Vector> &lambda) lambda) {
: gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {} }
/*****************************************************************************/ /*****************************************************************************/
void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const {
@ -67,7 +79,7 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const {
/* substract A*x */ /* substract A*x */
Vector Ax = Vector::Zero(r.rows(), 1); Vector Ax = Vector::Zero(r.rows(), 1);
multiply(x, Ax); multiply(x, Ax);
r -= Ax ; r -= Ax;
} }
/*****************************************************************************/ /*****************************************************************************/
@ -75,7 +87,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
/* implement A^T*(A*x), assume x and AtAx are pre-allocated */ /* implement A^T*(A*x), assume x and AtAx are pre-allocated */
// Build a VectorValues for Vector x // Build a VectorValues for Vector x
VectorValues vvX = buildVectorValues(x,keyInfo_); VectorValues vvX = buildVectorValues(x, keyInfo_);
// VectorValues form of A'Ax for multiplyHessianAdd // VectorValues form of A'Ax for multiplyHessianAdd
VectorValues vvAtAx; VectorValues vvAtAx;
@ -99,31 +111,33 @@ void GaussianFactorGraphSystem::getb(Vector &b) const {
} }
/**********************************************************************************/ /**********************************************************************************/
void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const { void GaussianFactorGraphSystem::leftPrecondition(const Vector &x,
Vector &y) const {
// For a preconditioner M = L*L^T // For a preconditioner M = L*L^T
// Calculate y = L^{-1} x // Calculate y = L^{-1} x
preconditioner_.solve(x, y); preconditioner_.solve(x, y);
} }
/**********************************************************************************/ /**********************************************************************************/
void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const { void GaussianFactorGraphSystem::rightPrecondition(const Vector &x,
Vector &y) const {
// For a preconditioner M = L*L^T // For a preconditioner M = L*L^T
// Calculate y = L^{-T} x // Calculate y = L^{-T} x
preconditioner_.transposeSolve(x, y); preconditioner_.transposeSolve(x, y);
} }
/**********************************************************************************/ /**********************************************************************************/
VectorValues buildVectorValues(const Vector &v, VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
const Ordering &ordering, const map<Key, size_t> & dimensions) {
const map<Key, size_t> & dimensions) {
VectorValues result; VectorValues result;
DenseIndex offset = 0; DenseIndex offset = 0;
for ( size_t i = 0 ; i < ordering.size() ; ++i ) { for (size_t i = 0; i < ordering.size(); ++i) {
const Key &key = ordering[i]; const Key &key = ordering[i];
map<Key, size_t>::const_iterator it = dimensions.find(key); map<Key, size_t>::const_iterator it = dimensions.find(key);
if ( it == dimensions.end() ) { if (it == dimensions.end()) {
throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions"); throw invalid_argument(
"buildVectorValues: inconsistent ordering and dimensions");
} }
const size_t dim = it->second; const size_t dim = it->second;
result.insert(key, v.segment(offset, dim)); result.insert(key, v.segment(offset, dim));
@ -137,7 +151,8 @@ VectorValues buildVectorValues(const Vector &v,
VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) {
VectorValues result; VectorValues result;
BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) { BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) {
result.insert(item.first, v.segment(item.second.colstart(), item.second.dim())); result.insert(item.first,
v.segment(item.second.colstart(), item.second.dim()));
} }
return result; return result;
} }

57
gtsam/linear/PCGSolver.h
View File

@ -1,20 +1,25 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/* /*
* PCGSolver.h * @file PCGSolver.h
* * @brief Preconditioned Conjugate Gradient Solver for linear systems
* Created on: Jan 31, 2012 * @date Jan 31, 2012
* Author: Yong-Dian Jian * @author Yong-Dian Jian
* @author Sungtae An
*/ */
#pragma once #pragma once
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/ConjugateGradientSolver.h> #include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/shared_ptr.hpp>
#include <algorithm>
#include <iosfwd>
#include <map>
#include <string> #include <string>
namespace gtsam { namespace gtsam {
@ -22,15 +27,19 @@ namespace gtsam {
class GaussianFactorGraph; class GaussianFactorGraph;
class KeyInfo; class KeyInfo;
class Preconditioner; class Preconditioner;
class VectorValues;
struct PreconditionerParameters; struct PreconditionerParameters;
/*****************************************************************************/ /**
* Parameters for PCG
*/
struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters { struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters {
public: public:
typedef ConjugateGradientParameters Base; typedef ConjugateGradientParameters Base;
typedef boost::shared_ptr<PCGSolverParameters> shared_ptr; typedef boost::shared_ptr<PCGSolverParameters> shared_ptr;
PCGSolverParameters() {} PCGSolverParameters() {
}
virtual void print(std::ostream &os) const; virtual void print(std::ostream &os) const;
@ -42,8 +51,9 @@ public:
boost::shared_ptr<PreconditionerParameters> preconditioner_; boost::shared_ptr<PreconditionerParameters> preconditioner_;
}; };
/*****************************************************************************/ /**
/* A virtual base class for the preconditioned conjugate gradient solver */ * A virtual base class for the preconditioned conjugate gradient solver
*/
class GTSAM_EXPORT PCGSolver: public IterativeSolver { class GTSAM_EXPORT PCGSolver: public IterativeSolver {
public: public:
typedef IterativeSolver Base; typedef IterativeSolver Base;
@ -57,7 +67,8 @@ protected:
public: public:
/* Interface to initialize a solver without a problem */ /* Interface to initialize a solver without a problem */
PCGSolver(const PCGSolverParameters &p); PCGSolver(const PCGSolverParameters &p);
virtual ~PCGSolver() {} virtual ~PCGSolver() {
}
using IterativeSolver::optimize; using IterativeSolver::optimize;
@ -67,7 +78,9 @@ public:
}; };
/*****************************************************************************/ /**
* System class needed for calling preconditionedConjugateGradient
*/
class GTSAM_EXPORT GaussianFactorGraphSystem { class GTSAM_EXPORT GaussianFactorGraphSystem {
public: public:
@ -97,13 +110,17 @@ public:
void getb(Vector &b) const; void getb(Vector &b) const;
}; };
/* utility functions */ /// @name utility functions
/**********************************************************************************/ /// @{
/// Create VectorValues from a Vector
VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
const std::map<Key, size_t> & dimensions); const std::map<Key, size_t> & dimensions);
/**********************************************************************************/ /// Create VectorValues from a Vector and a KeyInfo class
VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo); VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo);
/// @}
} }

128
gtsam/linear/SubgraphSolver.cpp
View File

@ -9,73 +9,81 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
#include <gtsam/linear/Errors.h> /**
#include <gtsam/linear/GaussianConditional.h> * @file SubgraphSolver.cpp
* @brief Subgraph Solver from IROS 2010
* @date 2010
* @author Frank Dellaert
* @author Yong Dian Jian
*/
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/iterative-inl.h> #include <gtsam/linear/iterative-inl.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphPreconditioner.h> #include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph-inl.h>
#include <gtsam/base/DSFVector.h> #include <gtsam/base/DSFVector.h>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <list>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters, const Ordering& ordering) SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg,
: parameters_(parameters), ordering_(ordering) const Parameters &parameters, const Ordering& ordering) :
{ parameters_(parameters), ordering_(ordering) {
initialize(gfg); initialize(gfg);
} }
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters &parameters, const Ordering& ordering) SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg,
: parameters_(parameters), ordering_(ordering) const Parameters &parameters, const Ordering& ordering) :
{ parameters_(parameters), ordering_(ordering) {
initialize(*jfg); initialize(*jfg);
} }
/**************************************************************************************************/ /**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering) SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
: parameters_(parameters), ordering_(ordering) { const GaussianFactorGraph &Ab2, const Parameters &parameters,
const Ordering& ordering) :
parameters_(parameters), ordering_(ordering) {
GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, 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 Ordering& ordering) const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters,
: parameters_(parameters), ordering_(ordering) { const Ordering& ordering) :
parameters_(parameters), ordering_(ordering) {
GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, 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 Parameters &parameters, const Ordering& ordering) : parameters_(parameters), ordering_(ordering) const GaussianFactorGraph &Ab2, 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, const Ordering& ordering) : const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters,
parameters_(parameters), ordering_(ordering) 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);
} }
@ -86,29 +94,33 @@ VectorValues SubgraphSolver::optimize(const VectorValues &initial) {
} }
/**************************************************************************************************/ /**************************************************************************************************/
void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) {
{ GaussianFactorGraph::shared_ptr Ab1 =
GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>(), boost::make_shared<GaussianFactorGraph>(), Ab2 = boost::make_shared<
Ab2 = boost::make_shared<GaussianFactorGraph>(); 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;
GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR); GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_,
VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(Rc1->optimize()); EliminateQR);
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 = boost::make_shared<VectorValues>(Rc1->optimize()); 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) {
const VariableIndex index(jfg); const VariableIndex index(jfg);
@ -116,39 +128,43 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
DSFVector D(n); DSFVector D(n);
GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph()); GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph());
size_t t = 0; size_t t = 0;
BOOST_FOREACH ( const GaussianFactor::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 ");
} }
bool augment = false ; bool augment = false;
/* check whether this factor should be augmented to the "tree" graph */ /* check whether this factor should be augmented to the "tree" graph */
if ( gf->keys().size() == 1 ) augment = true; if (gf->keys().size() == 1)
augment = true;
else { else {
const Key u = gf->keys()[0], v = gf->keys()[1], const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.findSet(u),
u_root = D.findSet(u), v_root = D.findSet(v); v_root = D.findSet(v);
if ( u_root != v_root ) { if (u_root != v_root) {
t++; augment = true ; t++;
augment = true;
D.makeUnionInPlace(u_root, v_root); D.makeUnionInPlace(u_root, v_root);
} }
} }
if ( augment ) At->push_back(gf); if (augment)
else Ac->push_back(gf); At->push_back(gf);
else
Ac->push_back(gf);
} }
return boost::tie(At, Ac); return boost::tie(At, Ac);
} }
/****************************************************************************/ /****************************************************************************/
VectorValues SubgraphSolver::optimize ( VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg,
const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
const KeyInfo &keyInfo, const VectorValues &initial) {
const std::map<Key, Vector> &lambda, return VectorValues();
const VectorValues &initial }
) { return VectorValues(); }
} // \namespace gtsam } // \namespace gtsam

104
gtsam/linear/SubgraphSolver.h
View File

@ -9,27 +9,37 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/**
* @file SubgraphSolver.h
* @brief Subgraph Solver from IROS 2010
* @date 2010
* @author Frank Dellaert
* @author Yong Dian Jian
*/
#pragma once #pragma once
#include <gtsam/linear/ConjugateGradientSolver.h> #include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/inference/Ordering.h>
#include <boost/tuple/tuple.hpp>
#include <iosfwd>
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
class GaussianFactorGraph; class GaussianFactorGraph;
class GaussianBayesNet; class GaussianBayesNet;
class SubgraphPreconditioner; class SubgraphPreconditioner;
class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters { class GTSAM_EXPORT SubgraphSolverParameters: public ConjugateGradientParameters {
public: public:
typedef ConjugateGradientParameters Base; typedef ConjugateGradientParameters Base;
SubgraphSolverParameters() : Base() {} SubgraphSolverParameters() :
void print() const { Base::print(); } Base() {
virtual void print(std::ostream &os) const { Base::print(os); } }
void print() const {
Base::print();
}
virtual void print(std::ostream &os) const {
Base::print(os);
}
}; };
/** /**
@ -53,8 +63,7 @@ public:
* *
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT SubgraphSolver: public IterativeSolver {
class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
public: public:
typedef SubgraphSolverParameters Parameters; typedef SubgraphSolverParameters Parameters;
@ -62,41 +71,64 @@ public:
protected: protected:
Parameters parameters_; Parameters parameters_;
Ordering ordering_; Ordering ordering_;
boost::shared_ptr<SubgraphPreconditioner> pc_; ///< preconditioner object boost::shared_ptr<SubgraphPreconditioner> pc_; ///< preconditioner object
public: public:
/* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */
SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters, const Ordering& ordering);
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 */ /// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG
SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering); SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters,
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering); const Ordering& ordering);
/// Shared pointer version
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
*/
SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2,
const Parameters &parameters, const Ordering& ordering);
/// Shared pointer version
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 boost::shared_ptr<GaussianBayesNet> &Rc1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering); SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering); const GaussianFactorGraph &Ab2, const Parameters &parameters,
const Ordering& ordering);
virtual ~SubgraphSolver() {} /// Shared pointer version
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
const boost::shared_ptr<GaussianFactorGraph> &Ab2,
const Parameters &parameters, const Ordering& ordering);
VectorValues optimize () ; /// Destructor
VectorValues optimize (const VectorValues &initial) ; virtual ~SubgraphSolver() {
}
/* interface to the nonlinear optimizer that the subclasses have to implement */ /// Optimize from zero
virtual VectorValues optimize ( VectorValues optimize();
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, /// Optimize from given initial values
const std::map<Key, Vector> &lambda, VectorValues optimize(const VectorValues &initial);
const VectorValues &initial
) ; /** Interface that IterativeSolver subclasses have to implement */
virtual VectorValues optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
const VectorValues &initial);
protected: protected:
void initialize(const GaussianFactorGraph &jfg); void initialize(const GaussianFactorGraph &jfg);
void initialize(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2); void initialize(const boost::shared_ptr<GaussianBayesNet> &Rc1,
const boost::shared_ptr<GaussianFactorGraph> &Ab2);
boost::tuple<boost::shared_ptr<GaussianFactorGraph>, boost::shared_ptr<GaussianFactorGraph> > boost::tuple<boost::shared_ptr<GaussianFactorGraph>,
splitGraph(const GaussianFactorGraph &gfg) ; boost::shared_ptr<GaussianFactorGraph> >
splitGraph(const GaussianFactorGraph &gfg);
}; };
} // namespace gtsam } // namespace gtsam

11
gtsam/slam/JacobianFactorQ.h
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/* /*
* @file JacobianFactorQ.h * @file JacobianFactorQ.h
* @date Oct 27, 2013 * @date Oct 27, 2013

7
gtsam/slam/JacobianFactorQR.h
View File

@ -7,8 +7,13 @@
#pragma once #pragma once
#include <gtsam/slam/JacobianSchurFactor.h> #include <gtsam/slam/JacobianSchurFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/Symbol.h>
namespace gtsam { namespace gtsam {
class GaussianBayesNet;
/** /**
* JacobianFactor for Schur complement that uses Q noise model * JacobianFactor for Schur complement that uses Q noise model
*/ */
@ -38,7 +43,7 @@ public:
//gfg.print("gfg"); //gfg.print("gfg");
// eliminate the point // eliminate the point
GaussianBayesNet::shared_ptr bn; boost::shared_ptr<GaussianBayesNet> bn;
GaussianFactorGraph::shared_ptr fg; GaussianFactorGraph::shared_ptr fg;
std::vector < Key > variables; std::vector < Key > variables;
variables.push_back(pointKey); variables.push_back(pointKey);

15
gtsam/slam/JacobianSchurFactor.h
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/* /*
* @file JacobianSchurFactor.h * @file JacobianSchurFactor.h
* @brief Jacobianfactor that combines and eliminates points * @brief Jacobianfactor that combines and eliminates points
@ -7,11 +18,7 @@
#pragma once #pragma once
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
namespace gtsam { namespace gtsam {

4
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -7,12 +7,10 @@
#pragma once #pragma once
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/make_shared.hpp> #include <iosfwd>
#include <iostream>
namespace gtsam { namespace gtsam {

39
gtsam/slam/RegularJacobianFactor.h
View File

@ -52,8 +52,8 @@ public:
* factor. */ * factor. */
template<typename KEYS> template<typename KEYS>
RegularJacobianFactor(const KEYS& keys, RegularJacobianFactor(const KEYS& keys,
const VerticalBlockMatrix& augmentedMatrix, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas =
const SharedDiagonal& sigmas = SharedDiagonal()) : SharedDiagonal()) :
JacobianFactor(keys, augmentedMatrix, sigmas) { JacobianFactor(keys, augmentedMatrix, sigmas) {
} }
@ -78,10 +78,10 @@ public:
/// Just iterate over all A matrices and multiply in correct config part (looping over keys) /// Just iterate over all A matrices and multiply in correct config part (looping over keys)
/// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'
/// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)
for (size_t pos = 0; pos < size(); ++pos) for (size_t pos = 0; pos < size(); ++pos) {
{
Ax += Ab_(pos) Ax += Ab_(pos)
* ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); * ConstMapD(x + accumulatedDims[keys_[pos]],
accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]);
} }
/// Deal with noise properly, need to Double* whiten as we are dividing by variance /// Deal with noise properly, need to Double* whiten as we are dividing by variance
if (model_) { if (model_) {
@ -93,8 +93,9 @@ public:
Ax *= alpha; Ax *= alpha;
/// Again iterate over all A matrices and insert Ai^T into y /// Again iterate over all A matrices and insert Ai^T into y
for (size_t pos = 0; pos < size(); ++pos){ for (size_t pos = 0; pos < size(); ++pos) {
MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( MapD(y + accumulatedDims[keys_[pos]],
accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
pos).transpose() * Ax; pos).transpose() * Ax;
} }
} }
@ -102,21 +103,25 @@ public:
/** Raw memory access version of multiplyHessianAdd */ /** Raw memory access version of multiplyHessianAdd */
void multiplyHessianAdd(double alpha, const double* x, double* y) const { void multiplyHessianAdd(double alpha, const double* x, double* y) const {
if (empty()) return; if (empty())
return;
Vector Ax = zero(Ab_.rows()); Vector Ax = zero(Ab_.rows());
// Just iterate over all A matrices and multiply in correct config part // Just iterate over all A matrices and multiply in correct config part
for(size_t pos=0; pos<size(); ++pos) for (size_t pos = 0; pos < size(); ++pos)
Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]); Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
// Deal with noise properly, need to Double* whiten as we are dividing by variance // Deal with noise properly, need to Double* whiten as we are dividing by variance
if (model_) { model_->whitenInPlace(Ax); model_->whitenInPlace(Ax); } if (model_) {
model_->whitenInPlace(Ax);
model_->whitenInPlace(Ax);
}
// multiply with alpha // multiply with alpha
Ax *= alpha; Ax *= alpha;
// Again iterate over all A matrices and insert Ai^e into y // Again iterate over all A matrices and insert Ai^e into y
for(size_t pos=0; pos<size(); ++pos) for (size_t pos = 0; pos < size(); ++pos)
DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax; DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
} }
@ -128,12 +133,12 @@ public:
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
// Get the diagonal block, and insert its diagonal // Get the diagonal block, and insert its diagonal
DVector dj; DVector dj;
for (size_t k = 0; k < D; ++k){ for (size_t k = 0; k < D; ++k) {
if (model_){ if (model_) {
Vector column_k = Ab_(j).col(k); Vector column_k = Ab_(j).col(k);
column_k = model_->whiten(column_k); column_k = model_->whiten(column_k);
dj(k) = dot(column_k, column_k); dj(k) = dot(column_k, column_k);
}else{ } else {
dj(k) = Ab_(j).col(k).squaredNorm(); dj(k) = Ab_(j).col(k).squaredNorm();
} }
} }
@ -156,13 +161,13 @@ public:
} }
// Just iterate over all A matrices // Just iterate over all A matrices
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
DVector dj; DVector dj;
// gradient -= A'*b/sigma^2 // gradient -= A'*b/sigma^2
// Computing with each column // Computing with each column
for (size_t k = 0; k < D; ++k){ for (size_t k = 0; k < D; ++k) {
Vector column_k = Ab_(j).col(k); Vector column_k = Ab_(j).col(k);
dj(k) = -1.0*dot(b, column_k); dj(k) = -1.0 * dot(b, column_k);
} }
DMap(d + D * j) += dj; DMap(d + D * j) += dj;
} }

20
gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -1,16 +1,24 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file testImplicitSchurFactor.cpp * @file testRegularImplicitSchurFactor.cpp
* @brief unit test implicit jacobian factors * @brief unit test implicit jacobian factors
* @author Frank Dellaert * @author Frank Dellaert
* @date Oct 20, 2013 * @date Oct 20, 2013
*/ */
//#include <gtsam_unstable/slam/ImplicitSchurFactor.h>
#include <gtsam/slam/RegularImplicitSchurFactor.h>
//#include <gtsam_unstable/slam/JacobianFactorQ.h>
#include <gtsam/slam/JacobianFactorQ.h> #include <gtsam/slam/JacobianFactorQ.h>
//#include "gtsam_unstable/slam/JacobianFactorQR.h" #include <gtsam/slam/JacobianFactorQR.h>
#include "gtsam/slam/JacobianFactorQR.h" #include <gtsam/slam/RegularImplicitSchurFactor.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>

17
gtsam/slam/tests/testRegularJacobianFactor.cpp
View File

@ -1,3 +1,14 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file testRegularJacobianFactor.cpp * @file testRegularJacobianFactor.cpp
* @brief unit test regular jacobian factors * @brief unit test regular jacobian factors
@ -5,13 +16,13 @@
* @date Nov 12, 2014 * @date Nov 12, 2014
*/ */
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/RegularJacobianFactor.h> #include <gtsam/slam/RegularJacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>

2
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file TestSmartProjectionPoseFactor.cpp * @file testSmartProjectionPoseFactor.cpp
* @brief Unit tests for ProjectionFactor Class * @brief Unit tests for ProjectionFactor Class
* @author Chris Beall * @author Chris Beall
* @author Luca Carlone * @author Luca Carlone