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>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<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
*
* Created on: Jun 4, 2014
* Author: Yong-Dian Jian
* @file ConjugateGradientSolver.cpp
* @brief Implementation of Conjugate Gradient solver for a linear system
* @author Yong-Dian Jian
* @author Sungtae An
* @date Nov 6, 2014
*/
#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);
if (s == "GTSAM") return ConjugateGradientParameters::GTSAM;
@ -43,6 +56,7 @@ ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTransla
return ConjugateGradientParameters::GTSAM;
}
/*****************************************************************************/
}

9
gtsam/linear/ConjugateGradientSolver.h
View File

@ -20,7 +20,6 @@
#pragma once
#include <gtsam/linear/IterativeSolver.h>
#include <iosfwd>
namespace gtsam {
@ -87,9 +86,8 @@ public:
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)
* 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.
@ -98,8 +96,9 @@ public:
* [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems,
* 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281.
*/
template <class S, class V>
V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters &parameters) {
template<class S, class V>
V preconditionedConjugateGradient(const S &system, const V &initial,
const ConjugateGradientParameters &parameters) {
V estimate, residual, direction, q1, q2;
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
* @brief
* @brief Some support classes for iterative solvers
* @date Sep 3, 2012
* @author Yong-Dian Jian
*/
@ -9,18 +20,22 @@
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/algorithm/string.hpp>
#include <boost/foreach.hpp>
#include <iostream>
#include <string>
using namespace std;
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 {
@ -29,78 +44,82 @@ void IterativeOptimizationParameters::print() const {
/*****************************************************************************/
void IterativeOptimizationParameters::print(ostream &os) const {
os << "IterativeOptimizationParameters:" << endl
<< "verbosity: " << verbosityTranslator(verbosity_) << endl;
os << "IterativeOptimizationParameters:" << endl << "verbosity: "
<< verbosityTranslator(verbosity_) << endl;
}
/*****************************************************************************/
ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) {
ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) {
p.print(os);
return os;
}
/*****************************************************************************/
IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const string &src) {
string s = src; boost::algorithm::to_upper(s);
if (s == "SILENT") return IterativeOptimizationParameters::SILENT;
else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY;
else if (s == "ERROR") return IterativeOptimizationParameters::ERROR;
IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(
const string &src) {
string s = src;
boost::algorithm::to_upper(s);
if (s == "SILENT")
return IterativeOptimizationParameters::SILENT;
else if (s == "COMPLEXITY")
return IterativeOptimizationParameters::COMPLEXITY;
else if (s == "ERROR")
return IterativeOptimizationParameters::ERROR;
/* default is default */
else return IterativeOptimizationParameters::SILENT;
else
return IterativeOptimizationParameters::SILENT;
}
/*****************************************************************************/
string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) {
if (verbosity == SILENT) return "SILENT";
else if (verbosity == COMPLEXITY) return "COMPLEXITY";
else if (verbosity == ERROR) return "ERROR";
else return "UNKNOWN";
string IterativeOptimizationParameters::verbosityTranslator(
IterativeOptimizationParameters::Verbosity verbosity) {
if (verbosity == SILENT)
return "SILENT";
else if (verbosity == COMPLEXITY)
return "COMPLEXITY";
else if (verbosity == ERROR)
return "ERROR";
else
return "UNKNOWN";
}
/*****************************************************************************/
VectorValues IterativeSolver::optimize(
const GaussianFactorGraph &gfg,
VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
boost::optional<const KeyInfo&> keyInfo,
boost::optional<const std::map<Key, Vector>&> lambda)
{
return optimize(
gfg,
keyInfo ? *keyInfo : KeyInfo(gfg),
lambda ? *lambda : std::map<Key,Vector>());
boost::optional<const std::map<Key, Vector>&> lambda) {
return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg),
lambda ? *lambda : std::map<Key, Vector>());
}
/*****************************************************************************/
VectorValues IterativeSolver::optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda)
{
VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) {
return optimize(gfg, keyInfo, lambda, keyInfo.x0());
}
/****************************************************************************/
KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering)
: ordering_(ordering) {
KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) :
ordering_(ordering) {
initialize(fg);
}
/****************************************************************************/
KeyInfo::KeyInfo(const GaussianFactorGraph &fg)
: ordering_(Ordering::Natural(fg)) {
KeyInfo::KeyInfo(const GaussianFactorGraph &fg) :
ordering_(Ordering::Natural(fg)) {
initialize(fg);
}
/****************************************************************************/
void KeyInfo::initialize(const GaussianFactorGraph &fg){
void KeyInfo::initialize(const GaussianFactorGraph &fg) {
const map<Key, size_t> colspec = fg.getKeyDimMap();
const size_t n = ordering_.size();
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 dim = colspec.find(key)->second;
insert(make_pair(key, KeyInfoEntry(i, dim, start)));
start += dim ;
start += dim;
}
numCols_ = start;
}
@ -108,7 +127,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg){
/****************************************************************************/
vector<size_t> KeyInfo::colSpec() const {
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();
}
return result;
@ -117,7 +136,7 @@ vector<size_t> KeyInfo::colSpec() const {
/****************************************************************************/
VectorValues KeyInfo::x0() const {
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()));
}
return result;
@ -128,7 +147,5 @@ Vector KeyInfo::x0vector() const {
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
#include <gtsam/base/Vector.h>
#include <gtsam/global_includes.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/Vector.h>
#include <boost/tuple/tuple.hpp>
#include <boost/optional/optional.hpp>
#include <boost/none.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/optional.hpp>
#include <iosfwd>
#include <map>
#include <string>
#include <vector>
#include <map>
namespace gtsam {
// Forward declarations
class KeyInfo;
class KeyInfoEntry;
class GaussianFactorGraph;
class Values;
class VectorValues;
// Forward declarations
class KeyInfo;
class KeyInfoEntry;
class GaussianFactorGraph;
class Values;
class VectorValues;
/************************************************************************************/
/**
* parameters for iterative linear solvers
*/
class GTSAM_EXPORT IterativeOptimizationParameters {
/**
* parameters for iterative linear solvers
*/
class GTSAM_EXPORT IterativeOptimizationParameters {
public:
public:
typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr;
enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_;
typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr;
enum Verbosity {
SILENT = 0, COMPLEXITY, ERROR
} verbosity_;
public:
public:
IterativeOptimizationParameters(Verbosity v = SILENT)
: verbosity_(v) {}
IterativeOptimizationParameters(Verbosity v = SILENT) :
verbosity_(v) {
}
virtual ~IterativeOptimizationParameters() {}
virtual ~IterativeOptimizationParameters() {
}
/* utility */
inline Verbosity verbosity() const { return verbosity_; }
std::string getVerbosity() const;
void setVerbosity(const std::string &s) ;
/* utility */
inline Verbosity verbosity() const {
return verbosity_;
}
std::string getVerbosity() const;
void setVerbosity(const std::string &s);
/* matlab interface */
void print() const ;
/* matlab interface */
void print() const;
/* virtual print function */
virtual void print(std::ostream &os) const ;
/* virtual print function */
virtual void print(std::ostream &os) const;
/* for serialization */
friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p);
/* for serialization */
friend std::ostream& operator<<(std::ostream &os,
const IterativeOptimizationParameters &p);
static Verbosity verbosityTranslator(const std::string &s);
static std::string verbosityTranslator(Verbosity v);
};
static Verbosity verbosityTranslator(const std::string &s);
static std::string verbosityTranslator(Verbosity v);
};
/************************************************************************************/
class GTSAM_EXPORT IterativeSolver {
public:
typedef boost::shared_ptr<IterativeSolver> shared_ptr;
IterativeSolver() {}
virtual ~IterativeSolver() {}
/**
* Base class for Iterative Solvers like SubgraphSolver
*/
class GTSAM_EXPORT IterativeSolver {
public:
typedef boost::shared_ptr<IterativeSolver> shared_ptr;
IterativeSolver() {
}
virtual ~IterativeSolver() {
}
/* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
VectorValues optimize (
const GaussianFactorGraph &gfg,
/* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
VectorValues optimize(const GaussianFactorGraph &gfg,
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 */
VectorValues optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda
);
/* interface to the nonlinear optimizer, without initial estimate */
VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda);
/* interface to the nonlinear optimizer that the subclasses have to implement */
virtual VectorValues optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda,
const VectorValues &initial
) = 0;
/* interface to the nonlinear optimizer that the subclasses have to implement */
virtual VectorValues optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
const VectorValues &initial) = 0;
};
};
/************************************************************************************/
/* Handy data structure for iterative solvers
* key to (index, dimension, colstart) */
class GTSAM_EXPORT KeyInfoEntry : public boost::tuple<size_t, size_t, size_t> {
public:
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>(); }
};
/**
* Handy data structure for iterative solvers
* key to (index, dimension, colstart)
*/
class GTSAM_EXPORT KeyInfoEntry: public boost::tuple<size_t, size_t, size_t> {
/************************************************************************************/
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);
public:
std::vector<size_t> colSpec() const ;
VectorValues x0() const;
Vector x0vector() const;
typedef boost::tuple<Key, size_t, Key> Base;
inline size_t numCols() const { return numCols_; }
inline const Ordering & ordering() const { return ordering_; }
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>();
}
};
protected:
/**
* Handy data structure for iterative solvers
*/
class GTSAM_EXPORT KeyInfo: public std::map<Key, KeyInfoEntry> {
void initialize(const GaussianFactorGraph &fg);
public:
Ordering ordering_;
size_t numCols_;
typedef std::map<Key, KeyInfoEntry> Base;
};
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
*
* Created on: Feb 14, 2012
* Author: Yong-Dian Jian
* Author: Sungtae An
* @file PCGSolver.cpp
* @brief Preconditioned Conjugate Gradient Solver for linear systems
* @date Feb 14, 2012
* @author Yong-Dian Jian
* @author Sungtae An
*/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include <algorithm>
#include <iostream>
#include <stdexcept>
@ -21,7 +36,7 @@ namespace gtsam {
/*****************************************************************************/
void PCGSolverParameters::print(ostream &os) const {
Base::print(os);
os << "PCGSolverParameters:" << endl;
os << "PCGSolverParameters:" << endl;
preconditioner_->print(os);
}
@ -32,30 +47,27 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) {
}
/*****************************************************************************/
VectorValues PCGSolver::optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda,
const VectorValues &initial)
{
VectorValues PCGSolver::optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
const VectorValues &initial) {
/* build preconditioner */
preconditioner_->build(gfg, keyInfo, lambda);
/* apply pcg */
const Vector sol = preconditionedConjugateGradient<GaussianFactorGraphSystem, Vector>(
GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda),
initial.vector(keyInfo.ordering()), parameters_);
GaussianFactorGraphSystem system(gfg, *preconditioner_, keyInfo, lambda);
Vector x0 = initial.vector(keyInfo.ordering());
const Vector sol = preconditionedConjugateGradient(system, x0, parameters_);
return buildVectorValues(sol, keyInfo);
}
/*****************************************************************************/
GaussianFactorGraphSystem::GaussianFactorGraphSystem(
const GaussianFactorGraph &gfg,
const Preconditioner &preconditioner,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda)
: gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {}
const GaussianFactorGraph &gfg, const Preconditioner &preconditioner,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) :
gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(
lambda) {
}
/*****************************************************************************/
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 */
Vector Ax = Vector::Zero(r.rows(), 1);
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 */
// Build a VectorValues for Vector x
VectorValues vvX = buildVectorValues(x,keyInfo_);
VectorValues vvX = buildVectorValues(x, keyInfo_);
// VectorValues form of A'Ax for multiplyHessianAdd
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
// Calculate y = L^{-1} x
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
// Calculate y = L^{-T} x
preconditioner_.transposeSolve(x, y);
}
/**********************************************************************************/
VectorValues buildVectorValues(const Vector &v,
const Ordering &ordering,
const map<Key, size_t> & dimensions) {
VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
const map<Key, size_t> & dimensions) {
VectorValues result;
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];
map<Key, size_t>::const_iterator it = dimensions.find(key);
if ( it == dimensions.end() ) {
throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions");
if (it == dimensions.end()) {
throw invalid_argument(
"buildVectorValues: inconsistent ordering and dimensions");
}
const size_t dim = it->second;
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 result;
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;
}

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
*
* Created on: Jan 31, 2012
* Author: Yong-Dian Jian
* @file PCGSolver.h
* @brief Preconditioned Conjugate Gradient Solver for linear systems
* @date Jan 31, 2012
* @author Yong-Dian Jian
* @author Sungtae An
*/
#pragma once
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/shared_ptr.hpp>
#include <algorithm>
#include <iosfwd>
#include <map>
#include <string>
namespace gtsam {
@ -22,15 +27,19 @@ namespace gtsam {
class GaussianFactorGraph;
class KeyInfo;
class Preconditioner;
class VectorValues;
struct PreconditionerParameters;
/*****************************************************************************/
/**
* Parameters for PCG
*/
struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters {
public:
typedef ConjugateGradientParameters Base;
typedef boost::shared_ptr<PCGSolverParameters> shared_ptr;
PCGSolverParameters() {}
PCGSolverParameters() {
}
virtual void print(std::ostream &os) const;
@ -42,8 +51,9 @@ public:
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 {
public:
typedef IterativeSolver Base;
@ -57,7 +67,8 @@ protected:
public:
/* Interface to initialize a solver without a problem */
PCGSolver(const PCGSolverParameters &p);
virtual ~PCGSolver() {}
virtual ~PCGSolver() {
}
using IterativeSolver::optimize;
@ -67,7 +78,9 @@ public:
};
/*****************************************************************************/
/**
* System class needed for calling preconditionedConjugateGradient
*/
class GTSAM_EXPORT GaussianFactorGraphSystem {
public:
@ -97,13 +110,17 @@ public:
void getb(Vector &b) const;
};
/* utility functions */
/**********************************************************************************/
/// @name utility functions
/// @{
/// Create VectorValues from a Vector
VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
const std::map<Key, size_t> & dimensions);
/**********************************************************************************/
/// Create VectorValues from a Vector and a KeyInfo class
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/iterative-inl.h>
#include <gtsam/linear/GaussianFactorGraph.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 <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <list>
using namespace std;
namespace gtsam {
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters, const Ordering& ordering)
: parameters_(parameters), ordering_(ordering)
{
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg,
const Parameters &parameters, const Ordering& ordering) :
parameters_(parameters), ordering_(ordering) {
initialize(gfg);
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters &parameters, const Ordering& ordering)
: parameters_(parameters), ordering_(ordering)
{
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg,
const Parameters &parameters, const Ordering& ordering) :
parameters_(parameters), ordering_(ordering) {
initialize(*jfg);
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering)
: parameters_(parameters), ordering_(ordering) {
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
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));
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1,
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters, const Ordering& ordering)
: parameters_(parameters), ordering_(ordering) {
const GaussianFactorGraph::shared_ptr &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, Ab2);
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2,
const Parameters &parameters, const Ordering& ordering) : parameters_(parameters), ordering_(ordering)
{
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
const GaussianFactorGraph &Ab2, const Parameters &parameters,
const Ordering& ordering) :
parameters_(parameters), ordering_(ordering) {
initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters, const Ordering& ordering) :
parameters_(parameters), ordering_(ordering)
{
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters,
const Ordering& ordering) :
parameters_(parameters), ordering_(ordering) {
initialize(Rc1, Ab2);
}
/**************************************************************************************************/
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);
}
@ -86,29 +94,33 @@ VectorValues SubgraphSolver::optimize(const VectorValues &initial) {
}
/**************************************************************************************************/
void SubgraphSolver::initialize(const GaussianFactorGraph &jfg)
{
GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>(),
Ab2 = boost::make_shared<GaussianFactorGraph>();
void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) {
GaussianFactorGraph::shared_ptr Ab1 =
boost::make_shared<GaussianFactorGraph>(), Ab2 = boost::make_shared<
GaussianFactorGraph>();
boost::tie(Ab1, Ab2) = splitGraph(jfg) ;
boost::tie(Ab1, Ab2) = splitGraph(jfg);
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);
VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(Rc1->optimize());
GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_,
EliminateQR);
VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(
Rc1->optimize());
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
/**************************************************************************************************/
void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2)
{
VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(Rc1->optimize());
void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1,
const GaussianFactorGraph::shared_ptr &Ab2) {
VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(
Rc1->optimize());
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) {
const VariableIndex index(jfg);
@ -116,39 +128,43 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
DSFVector D(n);
GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph());
GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph());
size_t t = 0;
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 ");
if (gf->keys().size() > 2) {
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 */
if ( gf->keys().size() == 1 ) augment = true;
if (gf->keys().size() == 1)
augment = true;
else {
const Key u = gf->keys()[0], v = gf->keys()[1],
u_root = D.findSet(u), v_root = D.findSet(v);
if ( u_root != v_root ) {
t++; augment = true ;
const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.findSet(u),
v_root = D.findSet(v);
if (u_root != v_root) {
t++;
augment = true;
D.makeUnionInPlace(u_root, v_root);
}
}
if ( augment ) At->push_back(gf);
else Ac->push_back(gf);
if (augment)
At->push_back(gf);
else
Ac->push_back(gf);
}
return boost::tie(At, Ac);
}
/****************************************************************************/
VectorValues SubgraphSolver::optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda,
const VectorValues &initial
) { return VectorValues(); }
VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
const VectorValues &initial) {
return VectorValues();
}
} // \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
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/inference/Ordering.h>
#include <boost/tuple/tuple.hpp>
#include <iosfwd>
namespace gtsam {
// Forward declarations
class GaussianFactorGraph;
class GaussianBayesNet;
class SubgraphPreconditioner;
// Forward declarations
class GaussianFactorGraph;
class GaussianBayesNet;
class SubgraphPreconditioner;
class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters {
class GTSAM_EXPORT SubgraphSolverParameters: public ConjugateGradientParameters {
public:
typedef ConjugateGradientParameters Base;
SubgraphSolverParameters() : Base() {}
void print() const { Base::print(); }
virtual void print(std::ostream &os) const { Base::print(os); }
SubgraphSolverParameters() :
Base() {
}
void print() const {
Base::print();
}
virtual void print(std::ostream &os) const {
Base::print(os);
}
};
/**
@ -53,8 +63,7 @@ public:
*
* \nosubgrouping
*/
class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
class GTSAM_EXPORT SubgraphSolver: public IterativeSolver {
public:
typedef SubgraphSolverParameters Parameters;
@ -62,41 +71,64 @@ public:
protected:
Parameters parameters_;
Ordering ordering_;
boost::shared_ptr<SubgraphPreconditioner> pc_; ///< preconditioner object
boost::shared_ptr<SubgraphPreconditioner> pc_; ///< preconditioner object
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 */
SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering);
SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering);
/// 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);
/// 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 */
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering);
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering);
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
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 () ;
VectorValues optimize (const VectorValues &initial) ;
/// Destructor
virtual ~SubgraphSolver() {
}
/* interface to the nonlinear optimizer that the subclasses have to implement */
virtual VectorValues optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda,
const VectorValues &initial
) ;
/// Optimize from zero
VectorValues optimize();
/// Optimize from given initial values
VectorValues optimize(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:
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> >
splitGraph(const GaussianFactorGraph &gfg) ;
boost::tuple<boost::shared_ptr<GaussianFactorGraph>,
boost::shared_ptr<GaussianFactorGraph> >
splitGraph(const GaussianFactorGraph &gfg);
};
} // 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
* @date Oct 27, 2013

7
gtsam/slam/JacobianFactorQR.h
View File

@ -7,8 +7,13 @@
#pragma once
#include <gtsam/slam/JacobianSchurFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/Symbol.h>
namespace gtsam {
class GaussianBayesNet;
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
@ -38,7 +43,7 @@ public:
//gfg.print("gfg");
// eliminate the point
GaussianBayesNet::shared_ptr bn;
boost::shared_ptr<GaussianBayesNet> bn;
GaussianFactorGraph::shared_ptr fg;
std::vector < Key > variables;
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
* @brief Jacobianfactor that combines and eliminates points
@ -7,11 +18,7 @@
#pragma once
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/foreach.hpp>
namespace gtsam {

4
gtsam/slam/RegularImplicitSchurFactor.h
View File

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

39
gtsam/slam/RegularJacobianFactor.h
View File

@ -52,8 +52,8 @@ public:
* factor. */
template<typename KEYS>
RegularJacobianFactor(const KEYS& keys,
const VerticalBlockMatrix& augmentedMatrix,
const SharedDiagonal& sigmas = SharedDiagonal()) :
const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas =
SharedDiagonal()) :
JacobianFactor(keys, augmentedMatrix, sigmas) {
}
@ -78,10 +78,10 @@ public:
/// 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]'
/// 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)
* 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
if (model_) {
@ -93,8 +93,9 @@ public:
Ax *= alpha;
/// Again iterate over all A matrices and insert Ai^T into y
for (size_t pos = 0; pos < size(); ++pos){
MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
for (size_t pos = 0; pos < size(); ++pos) {
MapD(y + accumulatedDims[keys_[pos]],
accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
pos).transpose() * Ax;
}
}
@ -102,21 +103,25 @@ public:
/** Raw memory access version of multiplyHessianAdd */
void multiplyHessianAdd(double alpha, const double* x, double* y) const {
if (empty()) return;
if (empty())
return;
Vector Ax = zero(Ab_.rows());
// 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]);
// 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
Ax *= alpha;
// 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;
}
@ -128,12 +133,12 @@ public:
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
// Get the diagonal block, and insert its diagonal
DVector dj;
for (size_t k = 0; k < D; ++k){
if (model_){
for (size_t k = 0; k < D; ++k) {
if (model_) {
Vector column_k = Ab_(j).col(k);
column_k = model_->whiten(column_k);
dj(k) = dot(column_k, column_k);
}else{
} else {
dj(k) = Ab_(j).col(k).squaredNorm();
}
}
@ -156,13 +161,13 @@ public:
}
// Just iterate over all A matrices
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
DVector dj;
// gradient -= A'*b/sigma^2
// 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);
dj(k) = -1.0*dot(b, column_k);
dj(k) = -1.0 * dot(b, column_k);
}
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
* @author Frank Dellaert
* @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_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/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
* @brief unit test regular jacobian factors
@ -5,13 +16,13 @@
* @date Nov 12, 2014
*/
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/RegularJacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.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
* @author Chris Beall
* @author Luca Carlone