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

Progress on unordered linear classes

release/4.3a0
Richard Roberts 2013-06-28 18:14:07 +00:00
parent 115768abc6
commit b1fdf32a8c
10 changed files with 394 additions and 473 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/linear/GaussianConditionalUnordered-inl.h
View File

@ -38,9 +38,15 @@ namespace gtsam {
BaseFactor(terms, d, sigmas), BaseConditional(nrFrontals) {} BaseFactor(terms, d, sigmas), BaseConditional(nrFrontals) {}
/* ************************************************************************* */ /* ************************************************************************* */
template<typename ITERATOR> template<typename KEYS, class MATRIX>
GaussianConditional::shared_ptr GaussianConditional::Combine(ITERATOR firstConditional, ITERATOR lastConditional) { GaussianConditionalUnordered::GaussianConditionalUnordered(
const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas) :
BaseFactor(keys, augmentedMatrix, sigmas), BaseConditional(nrFrontals) {}
/* ************************************************************************* */
template<typename ITERATOR>
GaussianConditional::shared_ptr GaussianConditional::Combine(ITERATOR firstConditional, ITERATOR lastConditional)
{
// TODO: check for being a clique // TODO: check for being a clique
// Get dimensions from first conditional // Get dimensions from first conditional

5
gtsam/linear/GaussianConditionalUnordered.cpp
View File

@ -134,7 +134,7 @@ namespace gtsam {
GaussianConditionalUnordered::const_iterator it; GaussianConditionalUnordered::const_iterator it;
for (it = beginParents(); it!= endParents(); it++) { for (it = beginParents(); it!= endParents(); it++) {
const Index i = *it; const Index i = *it;
transposeMultiplyAdd(-1.0,getA(it),frontalVec,gy[i]); gtsam::transposeMultiplyAdd(-1.0, getA(it), frontalVec, gy[i]);
} }
internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
} }
@ -142,7 +142,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianConditionalUnordered::scaleFrontalsBySigma(VectorValuesUnordered& gy) const { void GaussianConditionalUnordered::scaleFrontalsBySigma(VectorValuesUnordered& gy) const {
Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
frontalVec = emul(frontalVec, get_sigmas()); if(model_)
frontalVec.array() *= model_->sigmas().array();
internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals()); internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
} }

23
gtsam/linear/GaussianConditionalUnordered.h
View File

@ -52,33 +52,37 @@ namespace gtsam {
GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas); GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas);
/** constructor with only one parent /** constructor with only one parent
* |Rx+Sy-d| * |Rx+Sy-d| */
*/
GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, const SharedDiagonal& sigmas); Index name1, const Matrix& S, const SharedDiagonal& sigmas);
/** constructor with two parents /** constructor with two parents
* |Rx+Sy+Tz-d| * |Rx+Sy+Tz-d| */
*/
GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, Index name2, const Matrix& T, const SharedDiagonal& sigmas); Index name1, const Matrix& S, Index name2, const Matrix& T, const SharedDiagonal& sigmas);
/** /** Constructor with number of arbitrary parents. \f$ |Rx+sum(Ai*xi)-d| \f$
* Constructor with number of arbitrary parents. \f$ |Rx+sum(Ai*xi)-d| \f$
* @tparam PARENTS A container whose value type is std::pair<Key, Matrix>, specifying the * @tparam PARENTS A container whose value type is std::pair<Key, Matrix>, specifying the
* collection of parent keys and matrices. */ * collection of parent keys and matrices. */
template<typename PARENTS> template<typename PARENTS>
GaussianConditionalUnordered(Index key, const Vector& d, GaussianConditionalUnordered(Index key, const Vector& d,
const Matrix& R, const PARENTS& parents, const SharedDiagonal& sigmas); const Matrix& R, const PARENTS& parents, const SharedDiagonal& sigmas);
/** /** Constructor with arbitrary number of frontals and parents.
* Constructor with arbitrary number of frontals and parents.
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
* collection of keys and matrices making up the conditional. */ * collection of keys and matrices making up the conditional. */
template<typename TERMS> template<typename TERMS>
GaussianConditionalUnordered(const TERMS& terms, GaussianConditionalUnordered(const TERMS& terms,
size_t nrFrontals, const Vector& d, const SharedDiagonal& sigmas); size_t nrFrontals, const Vector& d, const SharedDiagonal& sigmas);
/** Constructor with arbitrary number keys, and where the augmented matrix is given all together
* instead of in block terms. Note that only the active view of the provided augmented matrix
* is used, and that the matrix data is copied into a newly-allocated matrix in the constructed
* factor. */
template<typename KEYS>
GaussianConditionalUnordered(
const KEYS& keys, size_t nrFrontals const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas);
/** Combine several GaussianConditional into a single dense GC. The conditionals enumerated by /** Combine several GaussianConditional into a single dense GC. The conditionals enumerated by
* \c first and \c last must be in increasing order, meaning that the parents of any * \c first and \c last must be in increasing order, meaning that the parents of any
* conditional may not include a conditional coming before it. * conditional may not include a conditional coming before it.
@ -99,10 +103,9 @@ namespace gtsam {
/** Return a view of the upper-triangular R block of the conditional */ /** Return a view of the upper-triangular R block of the conditional */
constABlock get_R() const { return Ab_.range(0, nrFrontals()); } constABlock get_R() const { return Ab_.range(0, nrFrontals()); }
/** Get a view of the parent block corresponding to the variable pointed to by the given key iterator (non-const version) */ /** Get a view of the parent blocks. */
constABlock get_S() const { return Ab_.range(nrFrontals(), size()); } constABlock get_S() const { return Ab_.range(nrFrontals(), size()); }
public:
/** /**
* Solves a conditional Gaussian and writes the solution into the entries of * Solves a conditional Gaussian and writes the solution into the entries of
* \c x for each frontal variable of the conditional. The parents are * \c x for each frontal variable of the conditional. The parents are

44
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -192,50 +192,6 @@ namespace gtsam {
augmented.col(augmented.cols()-1)); augmented.col(augmented.cols()-1));
} }
/* ************************************************************************* */
// Helper functions for Combine
static boost::tuple<vector<size_t>, size_t, size_t> countDims(const FactorGraph<JacobianFactor>& factors, const VariableSlots& variableSlots) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
#else
vector<size_t> varDims(variableSlots.size());
#endif
size_t m = 0;
size_t n = 0;
{
Index jointVarpos = 0;
BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) {
assert(slots.second.size() == factors.size());
Index sourceFactorI = 0;
BOOST_FOREACH(const Index sourceVarpos, slots.second) {
if(sourceVarpos < numeric_limits<Index>::max()) {
const JacobianFactor& sourceFactor = *factors[sourceFactorI];
size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
varDims[jointVarpos] = vardim;
n += vardim;
} else
assert(varDims[jointVarpos] == vardim);
#else
varDims[jointVarpos] = vardim;
n += vardim;
break;
#endif
}
++ sourceFactorI;
}
++ jointVarpos;
}
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) {
m += factor->rows();
}
}
return boost::make_tuple(varDims, m, n);
}
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::shared_ptr CombineJacobians( JacobianFactor::shared_ptr CombineJacobians(
const FactorGraph<JacobianFactor>& factors, const FactorGraph<JacobianFactor>& factors,

357
gtsam/linear/GaussianFactorGraphUnordered.cpp
View File

@ -18,15 +18,7 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <gtsam/base/Testable.h> #include <gtsam/linear/GaussianFactorGraphUnordered.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/cholesky.h> #include <gtsam/base/cholesky.h>
@ -37,63 +29,30 @@ using namespace gtsam;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : Base(CBN) {} GaussianFactorGraphUnordered::Keys GaussianFactorGraphUnordered::keys() const {
/* ************************************************************************* */
GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
FastSet<Index> keys; FastSet<Index> keys;
BOOST_FOREACH(const sharedFactor& factor, *this) BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor) keys.insert(factor->begin(), factor->end()); if (factor)
keys.insert(factor->begin(), factor->end());
return keys; return keys;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianFactorGraph::permuteWithInverse( GaussianFactorGraphUnordered GaussianFactorGraphUnordered::combine2(
const Permutation& inversePermutation) { const GaussianFactorGraphUnordered& lfg1, const GaussianFactorGraphUnordered& lfg2)
BOOST_FOREACH(const sharedFactor& factor, factors_) { {
if(factor) // Copy the first graph and add the second graph
factor->permuteWithInverse(inversePermutation); GaussianFactorGraphUnordered fg = lfg1;
} fg.push_back(lfg2);
}
/* ************************************************************************* */
void GaussianFactorGraph::reduceWithInverse(
const internal::Reduction& inverseReduction) {
BOOST_FOREACH(const sharedFactor& factor, factors_) {
if(factor)
factor->reduceWithInverse(inverseReduction);
}
}
/* ************************************************************************* */
void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg) {
for (const_iterator factor = lfg.factors_.begin(); factor
!= lfg.factors_.end(); factor++) {
push_back(*factor);
}
}
/* ************************************************************************* */
GaussianFactorGraph GaussianFactorGraph::combine2(
const GaussianFactorGraph& lfg1, const GaussianFactorGraph& lfg2) {
// create new linear factor graph equal to the first one
GaussianFactorGraph fg = lfg1;
// add the second factors_ in the graph
for (const_iterator factor = lfg2.factors_.begin(); factor
!= lfg2.factors_.end(); factor++) {
fg.push_back(*factor);
}
return fg; return fg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const { std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraphUnordered::sparseJacobian() const {
// First find dimensions of each variable // First find dimensions of each variable
FastVector<size_t> dims; FastVector<size_t> dims;
BOOST_FOREACH(const sharedFactor& factor, *this) { BOOST_FOREACH(const sharedFactor& factor, *this) {
for(GaussianFactor::const_iterator pos = factor->begin(); pos != factor->end(); ++pos) { for(GaussianFactorUnordered::const_iterator pos = factor->begin(); pos != factor->end(); ++pos) {
if(dims.size() <= *pos) if(dims.size() <= *pos)
dims.resize(*pos + 1, 0); dims.resize(*pos + 1, 0);
dims[*pos] = factor->getDim(pos); dims[*pos] = factor->getDim(pos);
@ -111,22 +70,23 @@ namespace gtsam {
size_t row = 0; size_t row = 0;
BOOST_FOREACH(const sharedFactor& factor, *this) { BOOST_FOREACH(const sharedFactor& factor, *this) {
// Convert to JacobianFactor if necessary // Convert to JacobianFactor if necessary
JacobianFactor::shared_ptr jacobianFactor( JacobianFactorUnordered::shared_ptr jacobianFactor(
boost::dynamic_pointer_cast<JacobianFactor>(factor)); boost::dynamic_pointer_cast<JacobianFactorUnordered>(factor));
if (!jacobianFactor) { if (!jacobianFactor) {
HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor)); //TODO Unordered: re-enable
if (hessian) //HessianFactorUnordered::shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactorUnordered>(factor));
jacobianFactor.reset(new JacobianFactor(*hessian)); //if (hessian)
else // jacobianFactor.reset(new JacobianFactorUnordered(*hessian));
//else
throw invalid_argument( throw invalid_argument(
"GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor."); "GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
} }
// Whiten the factor and add entries for it // Whiten the factor and add entries for it
// iterate over all variables in the factor // iterate over all variables in the factor
const JacobianFactor whitened(jacobianFactor->whiten()); const JacobianFactorUnordered whitened(jacobianFactor->whiten());
for(JacobianFactor::const_iterator pos=whitened.begin(); pos<whitened.end(); ++pos) { for(JacobianFactorUnordered::const_iterator pos=whitened.begin(); pos<whitened.end(); ++pos) {
JacobianFactor::constABlock whitenedA = whitened.getA(pos); JacobianFactorUnordered::constABlock whitenedA = whitened.getA(pos);
// find first column index for this key // find first column index for this key
size_t column_start = columnIndices[*pos]; size_t column_start = columnIndices[*pos];
for (size_t i = 0; i < (size_t) whitenedA.rows(); i++) for (size_t i = 0; i < (size_t) whitenedA.rows(); i++)
@ -137,7 +97,7 @@ namespace gtsam {
} }
} }
JacobianFactor::constBVector whitenedb(whitened.getb()); JacobianFactorUnordered::constBVector whitenedb(whitened.getb());
size_t bcolumn = columnIndices.back(); size_t bcolumn = columnIndices.back();
for (size_t i = 0; i < (size_t) whitenedb.size(); i++) for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
entries.push_back(boost::make_tuple(row+i, bcolumn, whitenedb(i))); entries.push_back(boost::make_tuple(row+i, bcolumn, whitenedb(i)));
@ -149,7 +109,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix GaussianFactorGraph::sparseJacobian_() const { Matrix GaussianFactorGraphUnordered::sparseJacobian_() const {
// call sparseJacobian // call sparseJacobian
typedef boost::tuple<size_t, size_t, double> triplet; typedef boost::tuple<size_t, size_t, double> triplet;
@ -168,24 +128,14 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix GaussianFactorGraph::augmentedJacobian() const { Matrix GaussianFactorGraphUnordered::augmentedJacobian() const {
// Convert to Jacobians
FactorGraph<JacobianFactor> jfg;
jfg.reserve(this->size());
BOOST_FOREACH(const sharedFactor& factor, *this) {
if(boost::shared_ptr<JacobianFactor> jf =
boost::dynamic_pointer_cast<JacobianFactor>(factor))
jfg.push_back(jf);
else
jfg.push_back(boost::make_shared<JacobianFactor>(*factor));
}
// combine all factors // combine all factors
JacobianFactor combined(*CombineJacobians(jfg, VariableSlots(*this))); JacobianFactorUnordered combined(*this);
return combined.matrix_augmented(); return combined.matrix_augmented();
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<Matrix,Vector> GaussianFactorGraph::jacobian() const { std::pair<Matrix,Vector> GaussianFactorGraphUnordered::jacobian() const {
Matrix augmented = augmentedJacobian(); Matrix augmented = augmentedJacobian();
return make_pair( return make_pair(
augmented.leftCols(augmented.cols()-1), augmented.leftCols(augmented.cols()-1),
@ -193,143 +143,14 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Helper functions for Combine GaussianFactorGraphUnordered::EliminationResult EliminateJacobians(const FactorGraph<
static boost::tuple<vector<size_t>, size_t, size_t> countDims(const FactorGraph<JacobianFactor>& factors, const VariableSlots& variableSlots) { JacobianFactorUnordered>& factors, size_t nrFrontals) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
#else
vector<size_t> varDims(variableSlots.size());
#endif
size_t m = 0;
size_t n = 0;
{
Index jointVarpos = 0;
BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) {
assert(slots.second.size() == factors.size());
Index sourceFactorI = 0;
BOOST_FOREACH(const Index sourceVarpos, slots.second) {
if(sourceVarpos < numeric_limits<Index>::max()) {
const JacobianFactor& sourceFactor = *factors[sourceFactorI];
size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
varDims[jointVarpos] = vardim;
n += vardim;
} else
assert(varDims[jointVarpos] == vardim);
#else
varDims[jointVarpos] = vardim;
n += vardim;
break;
#endif
}
++ sourceFactorI;
}
++ jointVarpos;
}
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) {
m += factor->rows();
}
}
return boost::make_tuple(varDims, m, n);
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::shared_ptr CombineJacobians( Matrix GaussianFactorGraphUnordered::augmentedHessian() const {
const FactorGraph<JacobianFactor>& factors,
const VariableSlots& variableSlots) {
const bool debug = ISDEBUG("CombineJacobians");
if (debug) factors.print("Combining factors: ");
if (debug) variableSlots.print();
if (debug) cout << "Determine dimensions" << endl;
gttic(countDims);
vector<size_t> varDims;
size_t m, n;
boost::tie(varDims, m, n) = countDims(factors, variableSlots);
if (debug) {
cout << "Dims: " << m << " x " << n << "\n";
BOOST_FOREACH(const size_t dim, varDims) cout << dim << " ";
cout << endl;
}
gttoc(countDims);
if (debug) cout << "Allocate new factor" << endl;
gttic(allocate);
JacobianFactor::shared_ptr combined(new JacobianFactor());
combined->allocate(variableSlots, varDims, m);
Vector sigmas(m);
gttoc(allocate);
if (debug) cout << "Copy blocks" << endl;
gttic(copy_blocks);
// Loop over slots in combined factor
Index combinedSlot = 0;
BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) {
JacobianFactor::ABlock destSlot(combined->getA(combined->begin()+combinedSlot));
// Loop over source factors
size_t nextRow = 0;
for(size_t factorI = 0; factorI < factors.size(); ++factorI) {
// Slot in source factor
const Index sourceSlot = varslot.second[factorI];
const size_t sourceRows = factors[factorI]->rows();
JacobianFactor::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows));
// Copy if exists in source factor, otherwise set zero
if(sourceSlot != numeric_limits<Index>::max())
destBlock = factors[factorI]->getA(factors[factorI]->begin()+sourceSlot);
else
destBlock.setZero();
nextRow += sourceRows;
}
++combinedSlot;
}
gttoc(copy_blocks);
if (debug) cout << "Copy rhs (b) and sigma" << endl;
gttic(copy_vectors);
bool anyConstrained = false;
// Loop over source factors
size_t nextRow = 0;
for(size_t factorI = 0; factorI < factors.size(); ++factorI) {
const size_t sourceRows = factors[factorI]->rows();
combined->getb().segment(nextRow, sourceRows) = factors[factorI]->getb();
sigmas.segment(nextRow, sourceRows) = factors[factorI]->get_model()->sigmas();
if (factors[factorI]->isConstrained()) anyConstrained = true;
nextRow += sourceRows;
}
gttoc(copy_vectors);
if (debug) cout << "Create noise model from sigmas" << endl;
gttic(noise_model);
combined->setModel(anyConstrained, sigmas);
gttoc(noise_model);
if (debug) cout << "Assert Invariants" << endl;
combined->assertInvariants();
return combined;
}
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph<
JacobianFactor>& factors, size_t nrFrontals) {
gttic(Combine);
JacobianFactor::shared_ptr jointFactor =
CombineJacobians(factors, VariableSlots(factors));
gttoc(Combine);
gttic(eliminate);
GaussianConditional::shared_ptr gbn = jointFactor->eliminate(nrFrontals);
gttoc(eliminate);
return make_pair(gbn, jointFactor);
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::augmentedHessian() const {
// combine all factors and get upper-triangular part of Hessian // combine all factors and get upper-triangular part of Hessian
HessianFactor combined(*this); HessianFactorUnordered combined(*this);
Matrix result = combined.info(); Matrix result = combined.info();
// Fill in lower-triangular part of Hessian // Fill in lower-triangular part of Hessian
result.triangularView<Eigen::StrictlyLower>() = result.transpose(); result.triangularView<Eigen::StrictlyLower>() = result.transpose();
@ -337,7 +158,7 @@ break;
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<Matrix,Vector> GaussianFactorGraph::hessian() const { std::pair<Matrix,Vector> GaussianFactorGraphUnordered::hessian() const {
Matrix augmented = augmentedHessian(); Matrix augmented = augmentedHessian();
return make_pair( return make_pair(
augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1), augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
@ -345,14 +166,14 @@ break;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< GaussianFactorGraphUnordered::EliminationResult EliminateCholesky(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals) { GaussianFactorUnordered>& factors, size_t nrFrontals) {
const bool debug = ISDEBUG("EliminateCholesky"); const bool debug = ISDEBUG("EliminateCholesky");
// Form Ab' * Ab // Form Ab' * Ab
gttic(combine); gttic(combine);
HessianFactor::shared_ptr combinedFactor(new HessianFactor(factors)); HessianFactorUnordered::shared_ptr combinedFactor(new HessianFactorUnordered(factors));
gttoc(combine); gttoc(combine);
// Do Cholesky, note that after this, the lower triangle still contains // Do Cholesky, note that after this, the lower triangle still contains
@ -378,17 +199,17 @@ break;
} }
/* ************************************************************************* */ /* ************************************************************************* */
static FactorGraph<JacobianFactor> convertToJacobians(const FactorGraph< static FactorGraph<JacobianFactorUnordered> convertToJacobians(const FactorGraph<
GaussianFactor>& factors) { GaussianFactorUnordered>& factors) {
typedef JacobianFactor J; typedef JacobianFactorUnordered J;
typedef HessianFactor H; typedef HessianFactorUnordered H;
const bool debug = ISDEBUG("convertToJacobians"); const bool debug = ISDEBUG("convertToJacobians");
FactorGraph<J> jacobians; FactorGraph<J> jacobians;
jacobians.reserve(factors.size()); jacobians.reserve(factors.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, factors)
if (factor) { if (factor) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor)); J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian) { if (jacobian) {
@ -403,7 +224,7 @@ break;
cout << "Converted HessianFactor to JacobianFactor:\n"; cout << "Converted HessianFactor to JacobianFactor:\n";
hessian->print("HessianFactor: "); hessian->print("HessianFactor: ");
converted->print("JacobianFactor: "); converted->print("JacobianFactor: ");
if (!assert_equal(*hessian, HessianFactor(*converted), 1e-3)) throw runtime_error( if (!assert_equal(*hessian, HessianFactorUnordered(*converted), 1e-3)) throw runtime_error(
"convertToJacobians: Conversion between Jacobian and Hessian incorrect"); "convertToJacobians: Conversion between Jacobian and Hessian incorrect");
} }
jacobians.push_back(converted); jacobians.push_back(converted);
@ -413,8 +234,18 @@ break;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph< GaussianFactorGraphUnordered::EliminationResult EliminateQR(
GaussianFactor>& factors, size_t nrFrontals) { const std::vector<GaussianFactorUnordered::shared_ptr>& factors, const std::vector<Key>& keys)
{
gttic(Combine);
JacobianFactorUnordered jointFactor(factors);
gttoc(Combine);
gttic(eliminate);
GaussianConditional::shared_ptr gbn = jointFactor->eliminate(nrFrontals);
gttoc(eliminate);
return make_pair(gbn, jointFactor);
const bool debug = ISDEBUG("EliminateQR"); const bool debug = ISDEBUG("EliminateQR");
@ -422,12 +253,12 @@ break;
if (debug) cout << "Using QR" << endl; if (debug) cout << "Using QR" << endl;
gttic(convert_to_Jacobian); gttic(convert_to_Jacobian);
FactorGraph<JacobianFactor> jacobians = convertToJacobians(factors); FactorGraph<JacobianFactorUnordered> jacobians = convertToJacobians(factors);
gttoc(convert_to_Jacobian); gttoc(convert_to_Jacobian);
gttic(Jacobian_EliminateGaussian); gttic(Jacobian_EliminateGaussian);
GaussianConditional::shared_ptr conditional; GaussianConditional::shared_ptr conditional;
GaussianFactor::shared_ptr factor; GaussianFactorUnordered::shared_ptr factor;
boost::tie(conditional, factor) = EliminateJacobians(jacobians, nrFrontals); boost::tie(conditional, factor) = EliminateJacobians(jacobians, nrFrontals);
gttoc(Jacobian_EliminateGaussian); gttoc(Jacobian_EliminateGaussian);
@ -435,9 +266,9 @@ break;
} // \EliminateQR } // \EliminateQR
/* ************************************************************************* */ /* ************************************************************************* */
bool hasConstraints(const FactorGraph<GaussianFactor>& factors) { bool hasConstraints(const FactorGraph<GaussianFactorUnordered>& factors) {
typedef JacobianFactor J; typedef JacobianFactorUnordered J;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, factors) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor)); J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model()->isConstrained()) { if (jacobian && jacobian->get_model()->isConstrained()) {
return true; return true;
@ -447,8 +278,8 @@ break;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminatePreferCholesky( GaussianFactorGraphUnordered::EliminationResult EliminatePreferCholesky(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) { const FactorGraph<GaussianFactorUnordered>& factors, size_t nrFrontals) {
// If any JacobianFactors have constrained noise models, we have to convert // If any JacobianFactors have constrained noise models, we have to convert
// all factors to JacobianFactors. Otherwise, we can convert all factors // all factors to JacobianFactors. Otherwise, we can convert all factors
@ -457,7 +288,7 @@ break;
if (hasConstraints(factors)) if (hasConstraints(factors))
return EliminateQR(factors, nrFrontals); return EliminateQR(factors, nrFrontals);
else { else {
GaussianFactorGraph::EliminationResult ret; GaussianFactorGraphUnordered::EliminationResult ret;
gttic(EliminateCholesky); gttic(EliminateCholesky);
ret = EliminateCholesky(factors, nrFrontals); ret = EliminateCholesky(factors, nrFrontals);
gttoc(EliminateCholesky); gttoc(EliminateCholesky);
@ -469,34 +300,34 @@ break;
/* ************************************************************************* */ /* ************************************************************************* */
static JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { static JacobianFactorUnordered::shared_ptr convertToJacobianFactorPtr(const GaussianFactorUnordered::shared_ptr &gf) {
JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactor>(gf); JacobianFactorUnordered::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactorUnordered>(gf);
if( !result ) { if( !result ) {
result = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) result = boost::make_shared<JacobianFactorUnordered>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
} }
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) { Errors operator*(const GaussianFactorGraphUnordered& fg, const VectorValues& x) {
Errors e; Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back((*Ai)*x); e.push_back((*Ai)*x);
} }
return e; return e;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e) { void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValues& x, Errors& e) {
multiplyInPlace(fg,x,e.begin()); multiplyInPlace(fg,x,e.begin());
} }
/* ************************************************************************* */ /* ************************************************************************* */
void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) { void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValues& x, const Errors::iterator& e) {
Errors::iterator ei = e; Errors::iterator ei = e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
*ei = (*Ai)*x; *ei = (*Ai)*x;
ei++; ei++;
} }
@ -504,22 +335,22 @@ break;
/* ************************************************************************* */ /* ************************************************************************* */
// x += alpha*A'*e // x += alpha*A'*e
void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) { void transposeMultiplyAdd(const GaussianFactorGraphUnordered& fg, double alpha, const Errors& e, VectorValues& x) {
// For each factor add the gradient contribution // For each factor add the gradient contribution
Errors::const_iterator ei = e.begin(); Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
Ai->transposeMultiplyAdd(alpha,*(ei++),x); Ai->transposeMultiplyAdd(alpha,*(ei++),x);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0) { VectorValues gradient(const GaussianFactorGraphUnordered& fg, const VectorValues& x0) {
// It is crucial for performance to make a zero-valued clone of x // It is crucial for performance to make a zero-valued clone of x
VectorValues g = VectorValues::Zero(x0); VectorValues g = VectorValues::Zero(x0);
Errors e; Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back(Ai->error_vector(x0)); e.push_back(Ai->error_vector(x0));
} }
transposeMultiplyAdd(fg, 1.0, e, g); transposeMultiplyAdd(fg, 1.0, e, g);
@ -527,22 +358,22 @@ break;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g) { void gradientAtZero(const GaussianFactorGraphUnordered& fg, VectorValues& g) {
// Zero-out the gradient // Zero-out the gradient
g.setZero(); g.setZero();
Errors e; Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back(-Ai->getb()); e.push_back(-Ai->getb());
} }
transposeMultiplyAdd(fg, 1.0, e, g); transposeMultiplyAdd(fg, 1.0, e, g);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { void residual(const GaussianFactorGraphUnordered& fg, const VectorValues &x, VectorValues &r) {
Index i = 0 ; Index i = 0 ;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
r[i] = Ai->getb(); r[i] = Ai->getb();
i++; i++;
} }
@ -552,13 +383,13 @@ break;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { void multiply(const GaussianFactorGraphUnordered& fg, const VectorValues &x, VectorValues &r) {
r.setZero(); r.setZero();
Index i = 0; Index i = 0;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
Vector &y = r[i]; Vector &y = r[i];
for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { for(JacobianFactorUnordered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
y += Ai->getA(j) * x[*j]; y += Ai->getA(j) * x[*j];
} }
++i; ++i;
@ -566,12 +397,12 @@ break;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) { void transposeMultiply(const GaussianFactorGraphUnordered& fg, const VectorValues &r, VectorValues &x) {
x.setZero(); x.setZero();
Index i = 0; Index i = 0;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { for(JacobianFactorUnordered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
x[*j] += Ai->getA(j).transpose() * r[i]; x[*j] += Ai->getA(j).transpose() * r[i];
} }
++i; ++i;
@ -579,10 +410,10 @@ break;
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x) { boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraphUnordered& fg, const VectorValues& x) {
boost::shared_ptr<Errors> e(new Errors); boost::shared_ptr<Errors> e(new Errors);
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e->push_back(Ai->error_vector(x)); e->push_back(Ai->error_vector(x));
} }
return e; return e;

176
gtsam/linear/GaussianFactorGraphUnordered.h
View File

@ -24,138 +24,111 @@
#include <gtsam/inference/FactorGraphUnordered.h> #include <gtsam/inference/FactorGraphUnordered.h>
#include <gtsam/inference/EliminateableFactorGraph.h> #include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/linear/GaussianFactorUnordered.h> #include <gtsam/linear/GaussianFactorUnordered.h>
#include <gtsam/linear/JacobianFactorUnordered.h>
namespace gtsam { namespace gtsam {
// Forward declaration to use as default argument, documented declaration below. // Forward declarations
GTSAM_EXPORT FactorGraph<GaussianFactor>::EliminationResult class GaussianFactorGraphUnordered;
EliminateQR(const FactorGraph<GaussianFactor>& factors, size_t nrFrontals); class GaussianFactorUnordered;
class GaussianConditionalUnordered;
class GaussianBayesNetUnordered;
class GaussianEliminationTreeUnordered;
class GaussianBayesTreeUnordered;
class GaussianJunctionTreeUnordered;
// Forward declaration to use as default elimination function, documented declaration below.
GTSAM_EXPORT
std::pair<boost::shared_ptr<GaussianConditionalUnordered>, boost::shared_ptr<GaussianFactorUnordered> >
EliminateQRUnordered(
const std::vector<boost::shared_ptr<GaussianFactorUnordered> >& factors, const std::vector<Key>& keys);
/* ************************************************************************* */
template<> class EliminationTraits<GaussianFactorGraphUnordered>
{
typedef GaussianFactorUnordered FactorType; ///< Type of factors in factor graph
typedef GaussianConditionalUnordered ConditionalType; ///< Type of conditionals from elimination
typedef GaussianBayesNetUnordered BayesNetType; ///< Type of Bayes net from sequential elimination
typedef GaussianEliminationTreeUnordered EliminationTreeType; ///< Type of elimination tree
typedef GaussianBayesTreeUnordered BayesTreeType; ///< Type of Bayes tree
typedef GaussianJunctionTreeUnordered JunctionTreeType; ///< Type of Junction tree
/// The default dense elimination function
static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
DefaultEliminate(const std::vector<boost::shared_ptr<FactorType> >& factors,
const std::vector<Key>& keys) { return EliminateQRUnordered(factors, keys); }
};
/* ************************************************************************* */
/** /**
* A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
* Factor == GaussianFactor * Factor == GaussianFactor
* VectorValues = A values structure of vectors * VectorValues = A values structure of vectors
* Most of the time, linear factor graphs arise by linearizing a non-linear factor graph. * Most of the time, linear factor graphs arise by linearizing a non-linear factor graph.
*/ */
class GaussianFactorGraph : public FactorGraph<GaussianFactor> { class GTSAM_EXPORT GaussianFactorGraphUnordered :
public FactorGraphUnordered<GaussianFactorUnordered>,
public EliminateableFactorGraph<GaussianFactorGraphUnordered>
{
public: public:
typedef boost::shared_ptr<GaussianFactorGraph> shared_ptr; typedef GaussianFactorGraphUnordered This; ///< Typedef to this class
typedef FactorGraph<GaussianFactor> Base; typedef FactorGraphUnordered<GaussianFactorUnordered> Base; ///< Typedef to base factor graph type
typedef EliminateableFactorGraph<This> BaseEliminateable; ///< Typedef to base elimination class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
/** /** Default constructor */
* Default constructor GaussianFactorGraphUnordered() {}
*/
GaussianFactorGraph() {}
/** /** Constructor that receives a BayesTree and returns a GaussianFactorGraph */
* Constructor that receives a Chordal Bayes Net and returns a GaussianFactorGraph GaussianFactorGraphUnordered(const GaussianBayesTreeUnordered& gbt) {
*/ push_back_bayesTree(gbt); }
GTSAM_EXPORT GaussianFactorGraph(const GaussianBayesNet& CBN);
/**
* Constructor that receives a BayesTree and returns a GaussianFactorGraph
*/
template<class CLIQUE>
GaussianFactorGraph(const BayesTree<GaussianConditional,CLIQUE>& gbt) : Base(gbt) {}
/** Constructor from a factor graph of GaussianFactor or a derived type */ /** Constructor from a factor graph of GaussianFactor or a derived type */
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& fg) { GaussianFactorGraphUnordered(const FactorGraphUnordered<DERIVEDFACTOR>& fg) : Base(fg.begin(), fg.end()) {}
push_back(fg);
}
/** Add a factor by value - makes a copy */ /** Add a factor by value - makes a copy */
void add(const GaussianFactor& factor) { void add(const GaussianFactorUnordered& factor) { factors_.push_back(factor.clone()); }
factors_.push_back(factor.clone());
}
/** Add a factor by pointer - stores pointer without copying the factor */ /** Add a factor by pointer - stores pointer without copying the factor */
void add(const sharedFactor& factor) { void add(const sharedFactor& factor) { factors_.push_back(factor); }
factors_.push_back(factor);
}
/** Add a null factor */ /** Add a null factor */
void add(const Vector& b) { void add(const Vector& b) {
add(JacobianFactor(b)); add(JacobianFactorUnordered(b)); }
}
/** Add a unary factor */ /** Add a unary factor */
void add(Index key1, const Matrix& A1, void add(Key key1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model) { const Vector& b, const SharedDiagonal& model) {
add(JacobianFactor(key1,A1,b,model)); add(JacobianFactorUnordered(key1,A1,b,model)); }
}
/** Add a binary factor */ /** Add a binary factor */
void add(Index key1, const Matrix& A1, void add(Key key1, const Matrix& A1,
Index key2, const Matrix& A2, Key key2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model) { const Vector& b, const SharedDiagonal& model) {
add(JacobianFactor(key1,A1,key2,A2,b,model)); add(JacobianFactorUnordered(key1,A1,key2,A2,b,model)); }
}
/** Add a ternary factor */ /** Add a ternary factor */
void add(Index key1, const Matrix& A1, void add(Key key1, const Matrix& A1,
Index key2, const Matrix& A2, Key key2, const Matrix& A2,
Index key3, const Matrix& A3, Key key3, const Matrix& A3,
const Vector& b, const SharedDiagonal& model) { const Vector& b, const SharedDiagonal& model) {
add(JacobianFactor(key1,A1,key2,A2,key3,A3,b,model)); add(JacobianFactorUnordered(key1,A1,key2,A2,key3,A3,b,model)); }
}
/** Add an n-ary factor */ /** Add an n-ary factor */
void add(const std::vector<std::pair<Index, Matrix> > &terms, template<class TERMS>
const Vector &b, const SharedDiagonal& model) { void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model) {
add(JacobianFactor(terms,b,model)); add(JacobianFactorUnordered(terms,b,model)); }
}
/** /**
* Return the set of variables involved in the factors (computes a set * Return the set of variables involved in the factors (computes a set
* union). * union).
*/ */
typedef FastSet<Index> Keys; typedef FastSet<Key> Keys;
GTSAM_EXPORT Keys keys() const; Keys keys() const;
/** Eliminate the first \c n frontal variables, returning the resulting
* conditional and remaining factor graph - this is very inefficient for
* eliminating all variables, to do that use EliminationTree or
* JunctionTree. Note that this version simply calls
* FactorGraph<GaussianFactor>::eliminateFrontals with EliminateQR as the
* eliminate function argument.
*/
std::pair<sharedConditional, GaussianFactorGraph> eliminateFrontals(size_t nFrontals, const Eliminate& function = EliminateQR) const {
return Base::eliminateFrontals(nFrontals, function); }
/** Factor the factor graph into a conditional and a remaining factor graph.
* Given the factor graph \f$ f(X) \f$, and \c variables to factorize out
* \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where
* \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is
* a probability density or likelihood, the factorization produces a
* conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$.
*
* For efficiency, this function treats the variables to eliminate
* \c variables as fully-connected, so produces a dense (fully-connected)
* conditional on all of the variables in \c variables, instead of a sparse
* BayesNet. If the variables are not fully-connected, it is more efficient
* to sequentially factorize multiple times.
* Note that this version simply calls
* FactorGraph<GaussianFactor>::eliminate with EliminateQR as the eliminate
* function argument.
*/
std::pair<sharedConditional, GaussianFactorGraph> eliminate(const std::vector<Index>& variables, const Eliminate& function = EliminateQR) const {
return Base::eliminate(variables, function); }
/** Eliminate a single variable, by calling GaussianFactorGraph::eliminate. */
std::pair<sharedConditional, GaussianFactorGraph> eliminateOne(Index variable, const Eliminate& function = EliminateQR) const {
return Base::eliminateOne(variable, function); }
/** Permute the variables in the factors */
GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation);
/** Apply a reduction, which is a remapping of variable indices. */
GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction);
/** unnormalized error */ /** unnormalized error */
double error(const VectorValues& x) const { double error(const VectorValuesUnordered& x) const {
double total_error = 0.; double total_error = 0.;
BOOST_FOREACH(const sharedFactor& factor, *this) BOOST_FOREACH(const sharedFactor& factor, *this)
total_error += factor->error(x); total_error += factor->error(x);
@ -163,7 +136,7 @@ namespace gtsam {
} }
/** Unnormalized probability. O(n) */ /** Unnormalized probability. O(n) */
double probPrime(const VectorValues& c) const { double probPrime(const VectorValuesUnordered& c) const {
return exp(-0.5 * error(c)); return exp(-0.5 * error(c));
} }
@ -173,28 +146,21 @@ namespace gtsam {
* @param lfg2 Linear factor graph * @param lfg2 Linear factor graph
* @return a new combined factor graph * @return a new combined factor graph
*/ */
GTSAM_EXPORT static GaussianFactorGraph combine2(const GaussianFactorGraph& lfg1, static This combine2(const This& lfg1, const This& lfg2);
const GaussianFactorGraph& lfg2);
/**
* combine two factor graphs
* @param *lfg Linear factor graph
*/
GTSAM_EXPORT void combine(const GaussianFactorGraph &lfg);
/** /**
* Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix, * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix,
* where i(k) and j(k) are the base 0 row and column indices, s(k) a double. * where i(k) and j(k) are the base 0 row and column indices, s(k) a double.
* The standard deviations are baked into A and b * The standard deviations are baked into A and b
*/ */
GTSAM_EXPORT std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian() const; std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian() const;
/** /**
* Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries
* such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
* The standard deviations are baked into A and b * The standard deviations are baked into A and b
*/ */
GTSAM_EXPORT Matrix sparseJacobian_() const; Matrix sparseJacobian_() const;
/** /**
* Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$
@ -203,7 +169,7 @@ namespace gtsam {
* \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
* GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
*/ */
GTSAM_EXPORT Matrix augmentedJacobian() const; Matrix augmentedJacobian() const;
/** /**
* Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$, * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
@ -212,7 +178,7 @@ namespace gtsam {
* GaussianFactorGraph::augmentedJacobian and * GaussianFactorGraph::augmentedJacobian and
* GaussianFactorGraph::sparseJacobian. * GaussianFactorGraph::sparseJacobian.
*/ */
GTSAM_EXPORT std::pair<Matrix,Vector> jacobian() const; std::pair<Matrix,Vector> jacobian() const;
/** /**
* Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
@ -225,7 +191,7 @@ namespace gtsam {
and the negative log-likelihood is and the negative log-likelihood is
\f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$. \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
*/ */
GTSAM_EXPORT Matrix augmentedHessian() const; Matrix augmentedHessian() const;
/** /**
* Return the dense Hessian \f$ \Lambda \f$ and information vector * Return the dense Hessian \f$ \Lambda \f$ and information vector
@ -233,7 +199,7 @@ namespace gtsam {
* is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also
* GaussianFactorGraph::augmentedHessian. * GaussianFactorGraph::augmentedHessian.
*/ */
GTSAM_EXPORT std::pair<Matrix,Vector> hessian() const; std::pair<Matrix,Vector> hessian() const;
private: private:
/** Serialization function */ /** Serialization function */

2
gtsam/linear/GaussianFactorUnordered.h
View File

@ -55,7 +55,7 @@ namespace gtsam {
virtual bool equals(const GaussianFactorUnordered& lf, double tol = 1e-9) const = 0; virtual bool equals(const GaussianFactorUnordered& lf, double tol = 1e-9) const = 0;
/** Print for testable */ /** Print for testable */
virtual double error(const VectorValues& c) const = 0; /** 0.5*(A*x-b)'*D*(A*x-b) */ virtual double error(const VectorValuesUnordered& c) const = 0; /** 0.5*(A*x-b)'*D*(A*x-b) */
/** Return the dimension of the variable pointed to by the given key iterator */ /** Return the dimension of the variable pointed to by the given key iterator */
virtual size_t getDim(const_iterator variable) const = 0; virtual size_t getDim(const_iterator variable) const = 0;

30
gtsam/linear/JacobianFactorUnordered-inl.h
View File

@ -32,6 +32,36 @@ namespace gtsam {
fillTerms(terms, b, model); fillTerms(terms, b, model);
} }
/* ************************************************************************* */
template<typename KEYS, class MATRIX>
JacobianFactorUnordered::JacobianFactorUnordered(
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas) :
Base(keys)
{
// Check noise model dimension
if(noiseModel && model->dim() != augmentedMatrix.rows())
throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim());
// Check number of variables
if(Base::keys_.size() != augmentedMatrix.nBlocks() - 1)
throw std::invalid_argument(
"Error in JacobianFactor constructor input. Number of provided keys plus\n"
"one for the RHS vector must equal the number of provided matrix blocks.");
// Check RHS dimension
if(augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1)
throw std::invalid_argument(
"Error in JacobianFactor constructor input. The last provided matrix block\n"
"must be the RHS vector, but the last provided block had more than one column.");
// Allocate and copy matrix - only takes the active view of the provided augmented matrix
Ab_ = VerticalBlockMatrix::LikeActiveViewOf(augmentedMatrix);
Ab_.full() = augmentedMatrix.full();
// Take noise model
model_ = model;
}
/* ************************************************************************* */ /* ************************************************************************* */
template<typename TERMS> template<typename TERMS>
void JacobianFactorUnordered::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel) void JacobianFactorUnordered::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel)

183
gtsam/linear/JacobianFactorUnordered.cpp
View File

@ -43,6 +43,7 @@
#ifdef __GNUC__ #ifdef __GNUC__
#pragma GCC diagnostic pop #pragma GCC diagnostic pop
#endif #endif
#include <boost/range/algorithm/copy.hpp>
#include <cmath> #include <cmath>
#include <sstream> #include <sstream>
@ -123,12 +124,61 @@ namespace gtsam {
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactorUnordered::JacobianFactorUnordered(const GaussianFactorGraphUnordered& gfg) // Helper functions for combine constructor
namespace {
boost::tuple<vector<size_t>, size_t, size_t> _countDims(
const std::vector<JacobianFactorUnordered::shared_ptr>& factors, const VariableSlots& variableSlots)
{ {
// Cast or convert to Jacobians gttic(countDims);
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
#else
vector<size_t> varDims(variableSlots.size());
#endif
size_t m = 0;
size_t n = 0;
{
Index jointVarpos = 0;
BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) {
assert(slots.second.size() == factors.size());
Index sourceFactorI = 0;
BOOST_FOREACH(const Index sourceVarpos, slots.second) {
if(sourceVarpos < numeric_limits<Index>::max()) {
const JacobianFactorUnordered& sourceFactor = *factors[sourceFactorI];
size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
varDims[jointVarpos] = vardim;
n += vardim;
} else
assert(varDims[jointVarpos] == vardim);
#else
varDims[jointVarpos] = vardim;
n += vardim;
break;
#endif
}
++ sourceFactorI;
}
++ jointVarpos;
}
BOOST_FOREACH(const JacobianFactorUnordered::shared_ptr& factor, factors) {
m += factor->rows();
}
}
return boost::make_tuple(varDims, m, n);
}
/* ************************************************************************* */
std::vector<JacobianFactorUnordered::shared_ptr>
_convertOrCastToJacobians(const GaussianFactorGraphUnordered& factors)
{
gttic(Convert_to_Jacobians);
std::vector<JacobianFactorUnordered::shared_ptr> jacobians; std::vector<JacobianFactorUnordered::shared_ptr> jacobians;
jacobians.reserve(gfg.size()); jacobians.reserve(factors.size());
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, gfg) { BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, factors) {
if(factor) { if(factor) {
if(JacobianFactorUnordered::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactorUnordered>(factor)) if(JacobianFactorUnordered::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactorUnordered>(factor))
jacobians.push_back(jf); jacobians.push_back(jf);
@ -136,8 +186,84 @@ namespace gtsam {
jacobians.push_back(boost::make_shared<JacobianFactorUnordered>(*factor)); jacobians.push_back(boost::make_shared<JacobianFactorUnordered>(*factor));
} }
} }
return jacobians;
}
}
*this = *CombineJacobians(jacobians, VariableSlots(jacobians)); /* ************************************************************************* */
JacobianFactorUnordered::JacobianFactorUnordered(
const GaussianFactorGraphUnordered& factors, boost::optional<const VariableSlots&> variableSlots)
{
gttic(JacobianFactorUnordered_combine_constructor);
// Compute VariableSlots if one was not provided
gttic(Compute_VariableSlots);
boost::optional<VariableSlots> computedVariableSlots;
if(!variableSlots) {
computedVariableSlots = VariableSlots(factors);
variableSlots = computedVariableSlots; // Binds reference, does not copy VariableSlots
}
gttoc(Compute_VariableSlots);
// Cast or convert to Jacobians
std::vector<JacobianFactorUnordered::shared_ptr> jacobians = _convertOrCastToJacobians(factors);
// Count dimensions
vector<size_t> varDims;
size_t m, n;
boost::tie(varDims, m, n) = _countDims(jacobians, *variableSlots);
gttic(allocate);
Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1>(1)), m); // Allocate augmented matrix
Base::keys_.resize(variableSlots->size());
boost::range::copy(*variableSlots | boost::adaptors::map_keys, Base::keys_.begin()); // Get variable keys from VariableSlots
gttoc(allocate);
gttic(copy_blocks);
// Loop over slots in combined factor
Index combinedSlot = 0;
BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) {
JacobianFactorUnordered::ABlock destSlot(this->getA(this->begin()+combinedSlot));
// Loop over source jacobians
size_t nextRow = 0;
for(size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
// Slot in source factor
const Index sourceSlot = varslot.second[factorI];
const size_t sourceRows = jacobians[factorI]->rows();
JacobianFactorUnordered::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows));
// Copy if exists in source factor, otherwise set zero
if(sourceSlot != numeric_limits<Index>::max())
destBlock = jacobians[factorI]->getA(jacobians[factorI]->begin()+sourceSlot);
else
destBlock.setZero();
nextRow += sourceRows;
}
++combinedSlot;
}
gttoc(copy_blocks);
gttic(copy_vectors);
bool anyConstrained = false;
boost::optional<Vector> sigmas;
// Loop over source jacobians
size_t nextRow = 0;
for(size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
const size_t sourceRows = jacobians[factorI]->rows();
this->getb().segment(nextRow, sourceRows) = jacobians[factorI]->getb();
if(jacobians[factorI]->get_model()) {
// If the factor has a noise model and we haven't yet allocated sigmas, allocate it.
if(!sigmas)
sigmas = Vector::Constant(m, 1.0);
sigmas->segment(nextRow, sourceRows) = jacobians[factorI]->get_model()->sigmas();
if (jacobians[factorI]->isConstrained())
anyConstrained = true;
}
nextRow += sourceRows;
}
gttoc(copy_vectors);
if(sigmas)
this->setModel(anyConstrained, *sigmas);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -300,61 +426,50 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditionalUnordered::shared_ptr JacobianFactorUnordered::splitConditional(size_t nrFrontals) { GaussianConditionalUnordered::shared_ptr JacobianFactorUnordered::splitConditional(size_t nrFrontals)
assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0); {
assert(size() >= nrFrontals); if(nrFrontals > size())
assertInvariants(); throw std::invalid_argument("Requesting to split more variables than exist using JacobianFactor::splitConditional");
const bool debug = ISDEBUG("JacobianFactor::splitConditional"); size_t frontalDim = Ab_.range(0, nrFrontals).cols();
if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl;
if(debug) this->print("Splitting JacobianFactor: ");
size_t frontalDim = Ab_.range(0,nrFrontals).cols();
// Check for singular factor // Check for singular factor
if(model_->dim() < frontalDim) if(model_->dim() < frontalDim)
throw IndeterminantLinearSystemException(this->keys().front()); throw IndeterminantLinearSystemException(this->keys().front());
// Extract conditional // Restrict the matrix to be in the first nrFrontals variables and create the conditional
gttic(cond_Rd); gttic(cond_Rd);
const DenseIndex originalRowEnd = Ab_.rowEnd();
// Restrict the matrix to be in the first nrFrontals variables
Ab_.rowEnd() = Ab_.rowStart() + frontalDim; Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
const Eigen::VectorBlock<const Vector> sigmas = model_->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart()); const Eigen::VectorBlock<const Vector> sigmas = model_->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart());
GaussianConditional::shared_ptr conditional(new GaussianConditional(begin(), end(), nrFrontals, Ab_, sigmas)); GaussianConditionalUnordered::shared_ptr conditional(boost::make_shared<GaussianConditionalUnordered>(
if(debug) conditional->print("Extracted conditional: "); Base::keys_, nrFrontals, Ab_, sigmas));
Ab_.rowStart() += frontalDim; Ab_.rowStart() += frontalDim;
Ab_.firstBlock() += nrFrontals; Ab_.firstBlock() += nrFrontals;
Ab_.rowEnd() = originalRowEnd;
gttoc(cond_Rd); gttoc(cond_Rd);
if(debug) conditional->print("Extracted conditional: ");
gttic(remaining_factor);
// Take lower-right block of Ab to get the new factor // Take lower-right block of Ab to get the new factor
Ab_.rowEnd() = model_->dim(); gttic(remaining_factor);
keys_.erase(begin(), begin() + nrFrontals); keys_.erase(begin(), begin() + nrFrontals);
// Set sigmas with the right model // Set sigmas with the right model
if(model_) {
if (model_->isConstrained()) if (model_->isConstrained())
model_ = noiseModel::Constrained::MixedSigmas(sub(model_->sigmas(), frontalDim, model_->dim())); model_ = noiseModel::Constrained::MixedSigmas(model_->sigmas().tail(model_->sigmas().size() - frontalDim));
else else
model_ = noiseModel::Diagonal::Sigmas(sub(model_->sigmas(), frontalDim, model_->dim())); model_ = noiseModel::Diagonal::Sigmas(model_->sigmas().tail(model_->sigmas().size() - frontalDim));
if(debug) this->print("Eliminated factor: "); }
assert(Ab_.rows() <= Ab_.cols()-1);
gttoc(remaining_factor); gttoc(remaining_factor);
if(debug) print("Eliminated factor: ");
assertInvariants();
return conditional; return conditional;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditionalUnordered::shared_ptr JacobianFactorUnordered::eliminate(size_t nrFrontals) { GaussianConditionalUnordered::shared_ptr JacobianFactorUnordered::eliminate(size_t nrFrontals) {
assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0); if(Ab_.rowStart() != 0 || Ab_.rowEnd() != (size_t) Ab_.matrix().rows() && Ab_.firstBlock() == 0);
assert(size() >= nrFrontals); if(nrFrontals > size())
throw std::invalid_argument("Requesting to eliminate more variables than exist using JacobianFactor::splitConditional");
assertInvariants(); assertInvariants();
const bool debug = ISDEBUG("JacobianFactor::eliminate"); const bool debug = ISDEBUG("JacobianFactor::eliminate");

17
gtsam/linear/JacobianFactorUnordered.h
View File

@ -119,11 +119,24 @@ namespace gtsam {
template<typename TERMS> template<typename TERMS>
JacobianFactorUnordered(const TERMS&terms, const Vector &b, const SharedDiagonal& model); JacobianFactorUnordered(const TERMS&terms, const Vector &b, const SharedDiagonal& model);
/** Constructor with arbitrary number keys, and where the augmented matrix is given all together
* instead of in block terms. Note that only the active view of the provided augmented matrix
* is used, and that the matrix data is copied into a newly-allocated matrix in the constructed
* factor. */
template<typename KEYS>
JacobianFactorUnordered(
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas);
/** Convert from a HessianFactor (does Cholesky) */ /** Convert from a HessianFactor (does Cholesky) */
//JacobianFactorUnordered(const HessianFactor& factor); //JacobianFactorUnordered(const HessianFactor& factor);
/** Build a dense joint factor from all the factors in a factor graph. */ /**
explicit JacobianFactorUnordered(const GaussianFactorGraphUnordered& gfg); * Build a dense joint factor from all the factors in a factor graph. If a VariableSlots
* structure computed for \c graph is already available, providing it will reduce the amount of
* computation performed. */
explicit JacobianFactorUnordered(
const GaussianFactorGraphUnordered& graph,
boost::optional<const VariableSlots&> variableSlots = boost::none);
/** Virtual destructor */ /** Virtual destructor */
virtual ~JacobianFactorUnordered() {} virtual ~JacobianFactorUnordered() {}