Progress on unordered linear classes
Richard Roberts
parent
115768abc6
commit
b1fdf32a8c
|
|
@ -38,9 +38,15 @@ namespace gtsam {
|
|||
BaseFactor(terms, d, sigmas), BaseConditional(nrFrontals) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename ITERATOR>
|
||||
GaussianConditional::shared_ptr GaussianConditional::Combine(ITERATOR firstConditional, ITERATOR lastConditional) {
|
||||
template<typename KEYS, class MATRIX>
|
||||
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
|
||||
|
||||
// Get dimensions from first conditional
|
||||
|
|
|
|||
|
|
@ -134,7 +134,7 @@ namespace gtsam {
|
|||
GaussianConditionalUnordered::const_iterator it;
|
||||
for (it = beginParents(); it!= endParents(); 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());
|
||||
}
|
||||
|
|
@ -142,7 +142,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
void GaussianConditionalUnordered::scaleFrontalsBySigma(VectorValuesUnordered& gy) const {
|
||||
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());
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -52,33 +52,37 @@ namespace gtsam {
|
|||
GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R, const SharedDiagonal& sigmas);
|
||||
|
||||
/** constructor with only one parent
|
||||
* |Rx+Sy-d|
|
||||
*/
|
||||
* |Rx+Sy-d| */
|
||||
GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R,
|
||||
Index name1, const Matrix& S, const SharedDiagonal& sigmas);
|
||||
|
||||
/** constructor with two parents
|
||||
* |Rx+Sy+Tz-d|
|
||||
*/
|
||||
* |Rx+Sy+Tz-d| */
|
||||
GaussianConditionalUnordered(Index key, const Vector& d, const Matrix& R,
|
||||
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
|
||||
* collection of parent keys and matrices. */
|
||||
template<typename PARENTS>
|
||||
GaussianConditionalUnordered(Index key, const Vector& d,
|
||||
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
|
||||
* collection of keys and matrices making up the conditional. */
|
||||
template<typename TERMS>
|
||||
GaussianConditionalUnordered(const TERMS& terms,
|
||||
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
|
||||
* \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.
|
||||
|
|
@ -99,10 +103,9 @@ namespace gtsam {
|
|||
/** Return a view of the upper-triangular R block of the conditional */
|
||||
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()); }
|
||||
|
||||
public:
|
||||
/**
|
||||
* Solves a conditional Gaussian and writes the solution into the entries of
|
||||
* \c x for each frontal variable of the conditional. The parents are
|
||||
|
|
|
|||
|
|
@ -192,50 +192,6 @@ namespace gtsam {
|
|||
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(
|
||||
const FactorGraph<JacobianFactor>& factors,
|
||||
|
|
|
|||
|
|
@ -18,15 +18,7 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Testable.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/linear/GaussianFactorGraphUnordered.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/cholesky.h>
|
||||
|
|
@ -37,63 +29,30 @@ using namespace gtsam;
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : Base(CBN) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
|
||||
GaussianFactorGraphUnordered::Keys GaussianFactorGraphUnordered::keys() const {
|
||||
FastSet<Index> keys;
|
||||
BOOST_FOREACH(const sharedFactor& factor, *this)
|
||||
if (factor) keys.insert(factor->begin(), factor->end());
|
||||
if (factor)
|
||||
keys.insert(factor->begin(), factor->end());
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianFactorGraph::permuteWithInverse(
|
||||
const Permutation& inversePermutation) {
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_) {
|
||||
if(factor)
|
||||
factor->permuteWithInverse(inversePermutation);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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);
|
||||
}
|
||||
GaussianFactorGraphUnordered GaussianFactorGraphUnordered::combine2(
|
||||
const GaussianFactorGraphUnordered& lfg1, const GaussianFactorGraphUnordered& lfg2)
|
||||
{
|
||||
// Copy the first graph and add the second graph
|
||||
GaussianFactorGraphUnordered fg = lfg1;
|
||||
fg.push_back(lfg2);
|
||||
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
|
||||
FastVector<size_t> dims;
|
||||
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)
|
||||
dims.resize(*pos + 1, 0);
|
||||
dims[*pos] = factor->getDim(pos);
|
||||
|
|
@ -111,22 +70,23 @@ namespace gtsam {
|
|||
size_t row = 0;
|
||||
BOOST_FOREACH(const sharedFactor& factor, *this) {
|
||||
// Convert to JacobianFactor if necessary
|
||||
JacobianFactor::shared_ptr jacobianFactor(
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(factor));
|
||||
JacobianFactorUnordered::shared_ptr jacobianFactor(
|
||||
boost::dynamic_pointer_cast<JacobianFactorUnordered>(factor));
|
||||
if (!jacobianFactor) {
|
||||
HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor));
|
||||
if (hessian)
|
||||
jacobianFactor.reset(new JacobianFactor(*hessian));
|
||||
else
|
||||
//TODO Unordered: re-enable
|
||||
//HessianFactorUnordered::shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactorUnordered>(factor));
|
||||
//if (hessian)
|
||||
// jacobianFactor.reset(new JacobianFactorUnordered(*hessian));
|
||||
//else
|
||||
throw invalid_argument(
|
||||
"GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
|
||||
}
|
||||
|
||||
// Whiten the factor and add entries for it
|
||||
// iterate over all variables in the factor
|
||||
const JacobianFactor whitened(jacobianFactor->whiten());
|
||||
for(JacobianFactor::const_iterator pos=whitened.begin(); pos<whitened.end(); ++pos) {
|
||||
JacobianFactor::constABlock whitenedA = whitened.getA(pos);
|
||||
const JacobianFactorUnordered whitened(jacobianFactor->whiten());
|
||||
for(JacobianFactorUnordered::const_iterator pos=whitened.begin(); pos<whitened.end(); ++pos) {
|
||||
JacobianFactorUnordered::constABlock whitenedA = whitened.getA(pos);
|
||||
// find first column index for this key
|
||||
size_t column_start = columnIndices[*pos];
|
||||
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();
|
||||
for (size_t i = 0; i < (size_t) whitenedb.size(); 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
|
||||
typedef boost::tuple<size_t, size_t, double> triplet;
|
||||
|
|
@ -168,168 +128,29 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix GaussianFactorGraph::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));
|
||||
}
|
||||
Matrix GaussianFactorGraphUnordered::augmentedJacobian() const {
|
||||
// combine all factors
|
||||
JacobianFactor combined(*CombineJacobians(jfg, VariableSlots(*this)));
|
||||
JacobianFactorUnordered combined(*this);
|
||||
return combined.matrix_augmented();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<Matrix,Vector> GaussianFactorGraph::jacobian() const {
|
||||
std::pair<Matrix,Vector> GaussianFactorGraphUnordered::jacobian() const {
|
||||
Matrix augmented = augmentedJacobian();
|
||||
return make_pair(
|
||||
augmented.leftCols(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);
|
||||
GaussianFactorGraphUnordered::EliminationResult EliminateJacobians(const FactorGraph<
|
||||
JacobianFactorUnordered>& factors, size_t nrFrontals) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::shared_ptr CombineJacobians(
|
||||
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 {
|
||||
Matrix GaussianFactorGraphUnordered::augmentedHessian() const {
|
||||
// combine all factors and get upper-triangular part of Hessian
|
||||
HessianFactor combined(*this);
|
||||
HessianFactorUnordered combined(*this);
|
||||
Matrix result = combined.info();
|
||||
// Fill in lower-triangular part of Hessian
|
||||
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();
|
||||
return make_pair(
|
||||
augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
|
||||
|
|
@ -345,14 +166,14 @@ break;
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
|
||||
GaussianFactor>& factors, size_t nrFrontals) {
|
||||
GaussianFactorGraphUnordered::EliminationResult EliminateCholesky(const FactorGraph<
|
||||
GaussianFactorUnordered>& factors, size_t nrFrontals) {
|
||||
|
||||
const bool debug = ISDEBUG("EliminateCholesky");
|
||||
|
||||
// Form Ab' * Ab
|
||||
gttic(combine);
|
||||
HessianFactor::shared_ptr combinedFactor(new HessianFactor(factors));
|
||||
HessianFactorUnordered::shared_ptr combinedFactor(new HessianFactorUnordered(factors));
|
||||
gttoc(combine);
|
||||
|
||||
// Do Cholesky, note that after this, the lower triangle still contains
|
||||
|
|
@ -378,17 +199,17 @@ break;
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static FactorGraph<JacobianFactor> convertToJacobians(const FactorGraph<
|
||||
GaussianFactor>& factors) {
|
||||
static FactorGraph<JacobianFactorUnordered> convertToJacobians(const FactorGraph<
|
||||
GaussianFactorUnordered>& factors) {
|
||||
|
||||
typedef JacobianFactor J;
|
||||
typedef HessianFactor H;
|
||||
typedef JacobianFactorUnordered J;
|
||||
typedef HessianFactorUnordered H;
|
||||
|
||||
const bool debug = ISDEBUG("convertToJacobians");
|
||||
|
||||
FactorGraph<J> jacobians;
|
||||
jacobians.reserve(factors.size());
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, factors)
|
||||
if (factor) {
|
||||
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
|
||||
if (jacobian) {
|
||||
|
|
@ -403,7 +224,7 @@ break;
|
|||
cout << "Converted HessianFactor to JacobianFactor:\n";
|
||||
hessian->print("HessianFactor: ");
|
||||
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");
|
||||
}
|
||||
jacobians.push_back(converted);
|
||||
|
|
@ -413,8 +234,18 @@ break;
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph<
|
||||
GaussianFactor>& factors, size_t nrFrontals) {
|
||||
GaussianFactorGraphUnordered::EliminationResult EliminateQR(
|
||||
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");
|
||||
|
||||
|
|
@ -422,12 +253,12 @@ break;
|
|||
if (debug) cout << "Using QR" << endl;
|
||||
|
||||
gttic(convert_to_Jacobian);
|
||||
FactorGraph<JacobianFactor> jacobians = convertToJacobians(factors);
|
||||
FactorGraph<JacobianFactorUnordered> jacobians = convertToJacobians(factors);
|
||||
gttoc(convert_to_Jacobian);
|
||||
|
||||
gttic(Jacobian_EliminateGaussian);
|
||||
GaussianConditional::shared_ptr conditional;
|
||||
GaussianFactor::shared_ptr factor;
|
||||
GaussianFactorUnordered::shared_ptr factor;
|
||||
boost::tie(conditional, factor) = EliminateJacobians(jacobians, nrFrontals);
|
||||
gttoc(Jacobian_EliminateGaussian);
|
||||
|
||||
|
|
@ -435,9 +266,9 @@ break;
|
|||
} // \EliminateQR
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool hasConstraints(const FactorGraph<GaussianFactor>& factors) {
|
||||
typedef JacobianFactor J;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
|
||||
bool hasConstraints(const FactorGraph<GaussianFactorUnordered>& factors) {
|
||||
typedef JacobianFactorUnordered J;
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, factors) {
|
||||
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
|
||||
if (jacobian && jacobian->get_model()->isConstrained()) {
|
||||
return true;
|
||||
|
|
@ -447,8 +278,8 @@ break;
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::EliminationResult EliminatePreferCholesky(
|
||||
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
|
||||
GaussianFactorGraphUnordered::EliminationResult EliminatePreferCholesky(
|
||||
const FactorGraph<GaussianFactorUnordered>& factors, size_t nrFrontals) {
|
||||
|
||||
// If any JacobianFactors have constrained noise models, we have to convert
|
||||
// all factors to JacobianFactors. Otherwise, we can convert all factors
|
||||
|
|
@ -457,7 +288,7 @@ break;
|
|||
if (hasConstraints(factors))
|
||||
return EliminateQR(factors, nrFrontals);
|
||||
else {
|
||||
GaussianFactorGraph::EliminationResult ret;
|
||||
GaussianFactorGraphUnordered::EliminationResult ret;
|
||||
gttic(EliminateCholesky);
|
||||
ret = EliminateCholesky(factors, nrFrontals);
|
||||
gttoc(EliminateCholesky);
|
||||
|
|
@ -469,34 +300,34 @@ break;
|
|||
|
||||
|
||||
/* ************************************************************************* */
|
||||
static JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) {
|
||||
JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
static JacobianFactorUnordered::shared_ptr convertToJacobianFactorPtr(const GaussianFactorUnordered::shared_ptr &gf) {
|
||||
JacobianFactorUnordered::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactorUnordered>(gf);
|
||||
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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) {
|
||||
Errors operator*(const GaussianFactorGraphUnordered& fg, const VectorValues& x) {
|
||||
Errors e;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
e.push_back((*Ai)*x);
|
||||
}
|
||||
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());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
*ei = (*Ai)*x;
|
||||
ei++;
|
||||
}
|
||||
|
|
@ -504,22 +335,22 @@ break;
|
|||
|
||||
/* ************************************************************************* */
|
||||
// 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
|
||||
Errors::const_iterator ei = e.begin();
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
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
|
||||
VectorValues g = VectorValues::Zero(x0);
|
||||
Errors e;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
e.push_back(Ai->error_vector(x0));
|
||||
}
|
||||
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
|
||||
g.setZero();
|
||||
Errors e;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
e.push_back(-Ai->getb());
|
||||
}
|
||||
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 ;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
r[i] = Ai->getb();
|
||||
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();
|
||||
Index i = 0;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
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];
|
||||
}
|
||||
++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();
|
||||
Index i = 0;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
for(JacobianFactorUnordered::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
|
||||
x[*j] += Ai->getA(j).transpose() * r[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_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
e->push_back(Ai->error_vector(x));
|
||||
}
|
||||
return e;
|
||||
|
|
|
|||
|
|
@ -24,138 +24,111 @@
|
|||
#include <gtsam/inference/FactorGraphUnordered.h>
|
||||
#include <gtsam/inference/EliminateableFactorGraph.h>
|
||||
#include <gtsam/linear/GaussianFactorUnordered.h>
|
||||
#include <gtsam/linear/JacobianFactorUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declaration to use as default argument, documented declaration below.
|
||||
GTSAM_EXPORT FactorGraph<GaussianFactor>::EliminationResult
|
||||
EliminateQR(const FactorGraph<GaussianFactor>& factors, size_t nrFrontals);
|
||||
// Forward declarations
|
||||
class GaussianFactorGraphUnordered;
|
||||
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.
|
||||
* Factor == GaussianFactor
|
||||
* VectorValues = A values structure of vectors
|
||||
* 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:
|
||||
|
||||
typedef boost::shared_ptr<GaussianFactorGraph> shared_ptr;
|
||||
typedef FactorGraph<GaussianFactor> Base;
|
||||
typedef GaussianFactorGraphUnordered This; ///< Typedef to this class
|
||||
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
|
||||
*/
|
||||
GaussianFactorGraph() {}
|
||||
/** Default constructor */
|
||||
GaussianFactorGraphUnordered() {}
|
||||
|
||||
/**
|
||||
* Constructor that receives a Chordal Bayes Net and returns a GaussianFactorGraph
|
||||
*/
|
||||
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 that receives a BayesTree and returns a GaussianFactorGraph */
|
||||
GaussianFactorGraphUnordered(const GaussianBayesTreeUnordered& gbt) {
|
||||
push_back_bayesTree(gbt); }
|
||||
|
||||
/** Constructor from a factor graph of GaussianFactor or a derived type */
|
||||
template<class DERIVEDFACTOR>
|
||||
GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& fg) {
|
||||
push_back(fg);
|
||||
}
|
||||
GaussianFactorGraphUnordered(const FactorGraphUnordered<DERIVEDFACTOR>& fg) : Base(fg.begin(), fg.end()) {}
|
||||
|
||||
/** Add a factor by value - makes a copy */
|
||||
void add(const GaussianFactor& factor) {
|
||||
factors_.push_back(factor.clone());
|
||||
}
|
||||
void add(const GaussianFactorUnordered& factor) { factors_.push_back(factor.clone()); }
|
||||
|
||||
/** Add a factor by pointer - stores pointer without copying the factor */
|
||||
void add(const sharedFactor& factor) {
|
||||
factors_.push_back(factor);
|
||||
}
|
||||
void add(const sharedFactor& factor) { factors_.push_back(factor); }
|
||||
|
||||
/** Add a null factor */
|
||||
void add(const Vector& b) {
|
||||
add(JacobianFactor(b));
|
||||
}
|
||||
add(JacobianFactorUnordered(b)); }
|
||||
|
||||
/** Add a unary factor */
|
||||
void add(Index key1, const Matrix& A1,
|
||||
void add(Key key1, const Matrix& A1,
|
||||
const Vector& b, const SharedDiagonal& model) {
|
||||
add(JacobianFactor(key1,A1,b,model));
|
||||
}
|
||||
add(JacobianFactorUnordered(key1,A1,b,model)); }
|
||||
|
||||
/** Add a binary factor */
|
||||
void add(Index key1, const Matrix& A1,
|
||||
Index key2, const Matrix& A2,
|
||||
void add(Key key1, const Matrix& A1,
|
||||
Key key2, const Matrix& A2,
|
||||
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 */
|
||||
void add(Index key1, const Matrix& A1,
|
||||
Index key2, const Matrix& A2,
|
||||
Index key3, const Matrix& A3,
|
||||
void add(Key key1, const Matrix& A1,
|
||||
Key key2, const Matrix& A2,
|
||||
Key key3, const Matrix& A3,
|
||||
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 */
|
||||
void add(const std::vector<std::pair<Index, Matrix> > &terms,
|
||||
const Vector &b, const SharedDiagonal& model) {
|
||||
add(JacobianFactor(terms,b,model));
|
||||
}
|
||||
template<class TERMS>
|
||||
void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model) {
|
||||
add(JacobianFactorUnordered(terms,b,model)); }
|
||||
|
||||
/**
|
||||
* Return the set of variables involved in the factors (computes a set
|
||||
* union).
|
||||
*/
|
||||
typedef FastSet<Index> Keys;
|
||||
GTSAM_EXPORT 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);
|
||||
typedef FastSet<Key> Keys;
|
||||
Keys keys() const;
|
||||
|
||||
/** unnormalized error */
|
||||
double error(const VectorValues& x) const {
|
||||
double error(const VectorValuesUnordered& x) const {
|
||||
double total_error = 0.;
|
||||
BOOST_FOREACH(const sharedFactor& factor, *this)
|
||||
total_error += factor->error(x);
|
||||
|
|
@ -163,7 +136,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** Unnormalized probability. O(n) */
|
||||
double probPrime(const VectorValues& c) const {
|
||||
double probPrime(const VectorValuesUnordered& c) const {
|
||||
return exp(-0.5 * error(c));
|
||||
}
|
||||
|
||||
|
|
@ -173,28 +146,21 @@ namespace gtsam {
|
|||
* @param lfg2 Linear factor graph
|
||||
* @return a new combined factor graph
|
||||
*/
|
||||
GTSAM_EXPORT static GaussianFactorGraph combine2(const GaussianFactorGraph& lfg1,
|
||||
const GaussianFactorGraph& lfg2);
|
||||
|
||||
/**
|
||||
* combine two factor graphs
|
||||
* @param *lfg Linear factor graph
|
||||
*/
|
||||
GTSAM_EXPORT void combine(const GaussianFactorGraph &lfg);
|
||||
static This combine2(const This& lfg1, const This& lfg2);
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* 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
|
||||
* 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
|
||||
*/
|
||||
GTSAM_EXPORT Matrix sparseJacobian_() const;
|
||||
Matrix sparseJacobian_() const;
|
||||
|
||||
/**
|
||||
* 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
|
||||
* 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$,
|
||||
|
|
@ -212,7 +178,7 @@ namespace gtsam {
|
|||
* GaussianFactorGraph::augmentedJacobian and
|
||||
* 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
|
||||
|
|
@ -225,7 +191,7 @@ namespace gtsam {
|
|||
and the negative log-likelihood is
|
||||
\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
|
||||
|
|
@ -233,7 +199,7 @@ namespace gtsam {
|
|||
* is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also
|
||||
* GaussianFactorGraph::augmentedHessian.
|
||||
*/
|
||||
GTSAM_EXPORT std::pair<Matrix,Vector> hessian() const;
|
||||
std::pair<Matrix,Vector> hessian() const;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -55,7 +55,7 @@ namespace gtsam {
|
|||
virtual bool equals(const GaussianFactorUnordered& lf, double tol = 1e-9) const = 0;
|
||||
|
||||
/** 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 */
|
||||
virtual size_t getDim(const_iterator variable) const = 0;
|
||||
|
|
|
|||
|
|
@ -32,6 +32,36 @@ namespace gtsam {
|
|||
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>
|
||||
void JacobianFactorUnordered::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel)
|
||||
|
|
|
|||
|
|
@ -43,6 +43,7 @@
|
|||
#ifdef __GNUC__
|
||||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
#include <boost/range/algorithm/copy.hpp>
|
||||
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
|
|
@ -123,21 +124,146 @@ namespace gtsam {
|
|||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactorUnordered::JacobianFactorUnordered(const GaussianFactorGraphUnordered& gfg)
|
||||
{
|
||||
// Cast or convert to Jacobians
|
||||
std::vector<JacobianFactorUnordered::shared_ptr> jacobians;
|
||||
jacobians.reserve(gfg.size());
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, gfg) {
|
||||
if(factor) {
|
||||
if(JacobianFactorUnordered::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactorUnordered>(factor))
|
||||
jacobians.push_back(jf);
|
||||
else
|
||||
jacobians.push_back(boost::make_shared<JacobianFactorUnordered>(*factor));
|
||||
// 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)
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
*this = *CombineJacobians(jacobians, VariableSlots(jacobians));
|
||||
/* ************************************************************************* */
|
||||
std::vector<JacobianFactorUnordered::shared_ptr>
|
||||
_convertOrCastToJacobians(const GaussianFactorGraphUnordered& factors)
|
||||
{
|
||||
gttic(Convert_to_Jacobians);
|
||||
std::vector<JacobianFactorUnordered::shared_ptr> jacobians;
|
||||
jacobians.reserve(factors.size());
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, factors) {
|
||||
if(factor) {
|
||||
if(JacobianFactorUnordered::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactorUnordered>(factor))
|
||||
jacobians.push_back(jf);
|
||||
else
|
||||
jacobians.push_back(boost::make_shared<JacobianFactorUnordered>(*factor));
|
||||
}
|
||||
}
|
||||
return 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) {
|
||||
assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0);
|
||||
assert(size() >= nrFrontals);
|
||||
assertInvariants();
|
||||
GaussianConditionalUnordered::shared_ptr JacobianFactorUnordered::splitConditional(size_t nrFrontals)
|
||||
{
|
||||
if(nrFrontals > size())
|
||||
throw std::invalid_argument("Requesting to split more variables than exist using JacobianFactor::splitConditional");
|
||||
|
||||
const bool debug = ISDEBUG("JacobianFactor::splitConditional");
|
||||
|
||||
if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl;
|
||||
if(debug) this->print("Splitting JacobianFactor: ");
|
||||
|
||||
size_t frontalDim = Ab_.range(0,nrFrontals).cols();
|
||||
size_t frontalDim = Ab_.range(0, nrFrontals).cols();
|
||||
|
||||
// Check for singular factor
|
||||
if(model_->dim() < frontalDim)
|
||||
throw IndeterminantLinearSystemException(this->keys().front());
|
||||
|
||||
// Extract conditional
|
||||
// Restrict the matrix to be in the first nrFrontals variables and create the conditional
|
||||
gttic(cond_Rd);
|
||||
|
||||
// Restrict the matrix to be in the first nrFrontals variables
|
||||
const DenseIndex originalRowEnd = Ab_.rowEnd();
|
||||
Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
|
||||
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));
|
||||
if(debug) conditional->print("Extracted conditional: ");
|
||||
GaussianConditionalUnordered::shared_ptr conditional(boost::make_shared<GaussianConditionalUnordered>(
|
||||
Base::keys_, nrFrontals, Ab_, sigmas));
|
||||
Ab_.rowStart() += frontalDim;
|
||||
Ab_.firstBlock() += nrFrontals;
|
||||
Ab_.rowEnd() = originalRowEnd;
|
||||
gttoc(cond_Rd);
|
||||
|
||||
if(debug) conditional->print("Extracted conditional: ");
|
||||
|
||||
gttic(remaining_factor);
|
||||
// Take lower-right block of Ab to get the new factor
|
||||
Ab_.rowEnd() = model_->dim();
|
||||
gttic(remaining_factor);
|
||||
keys_.erase(begin(), begin() + nrFrontals);
|
||||
// Set sigmas with the right model
|
||||
if (model_->isConstrained())
|
||||
model_ = noiseModel::Constrained::MixedSigmas(sub(model_->sigmas(), frontalDim, model_->dim()));
|
||||
else
|
||||
model_ = noiseModel::Diagonal::Sigmas(sub(model_->sigmas(), frontalDim, model_->dim()));
|
||||
if(debug) this->print("Eliminated factor: ");
|
||||
assert(Ab_.rows() <= Ab_.cols()-1);
|
||||
if(model_) {
|
||||
if (model_->isConstrained())
|
||||
model_ = noiseModel::Constrained::MixedSigmas(model_->sigmas().tail(model_->sigmas().size() - frontalDim));
|
||||
else
|
||||
model_ = noiseModel::Diagonal::Sigmas(model_->sigmas().tail(model_->sigmas().size() - frontalDim));
|
||||
}
|
||||
gttoc(remaining_factor);
|
||||
|
||||
if(debug) print("Eliminated factor: ");
|
||||
|
||||
assertInvariants();
|
||||
|
||||
return conditional;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianConditionalUnordered::shared_ptr JacobianFactorUnordered::eliminate(size_t nrFrontals) {
|
||||
|
||||
assert(Ab_.rowStart() == 0 && Ab_.rowEnd() == (size_t) matrix_.rows() && Ab_.firstBlock() == 0);
|
||||
assert(size() >= nrFrontals);
|
||||
if(Ab_.rowStart() != 0 || Ab_.rowEnd() != (size_t) Ab_.matrix().rows() && Ab_.firstBlock() == 0);
|
||||
if(nrFrontals > size())
|
||||
throw std::invalid_argument("Requesting to eliminate more variables than exist using JacobianFactor::splitConditional");
|
||||
assertInvariants();
|
||||
|
||||
const bool debug = ISDEBUG("JacobianFactor::eliminate");
|
||||
|
|
|
|||
|
|
@ -118,12 +118,25 @@ namespace gtsam {
|
|||
* collection of keys and matrices making up the factor. */
|
||||
template<typename TERMS>
|
||||
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) */
|
||||
//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 ~JacobianFactorUnordered() {}
|
||||
|
|
|
|||
Loading…
Reference in New Issue