Fixed errors and warnings in new unordered code
Richard Roberts
parent
67f3109e75
commit
4fa8332c77
|
|
@ -29,6 +29,7 @@ namespace gtsam {
|
|||
result.matrix_.resize(rhs.rows(), result.variableColOffsets_.back());
|
||||
result.rowEnd_ = rhs.rows();
|
||||
result.assertInvariants();
|
||||
return result;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -70,11 +70,11 @@ namespace gtsam {
|
|||
* Construct from a container of the sizes of each vertical block, resize the matrix so that its
|
||||
* height is matrixNewHeight and its width fits the given block dimensions. */
|
||||
template<typename CONTAINER>
|
||||
VerticalBlockMatrix(const CONTAINER dimensions, int height) :
|
||||
matrix_(matrixNewHeight, variableColOffsets_.back()),
|
||||
rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0)
|
||||
VerticalBlockMatrix(const CONTAINER dimensions, DenseIndex height) :
|
||||
matrix_(height, variableColOffsets_.back()),
|
||||
rowStart_(0), rowEnd_(height), blockStart_(0)
|
||||
{
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
fillOffsets(dimensions.begin(), dimensions.end());
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
|
|
@ -82,9 +82,9 @@ namespace gtsam {
|
|||
* Construct from iterator over the sizes of each vertical block, resize the matrix so that its
|
||||
* height is matrixNewHeight and its width fits the given block dimensions. */
|
||||
template<typename ITERATOR>
|
||||
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, int height) :
|
||||
matrix_(matrixNewHeight, variableColOffsets_.back()),
|
||||
rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0)
|
||||
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height) :
|
||||
matrix_(height, variableColOffsets_.back()),
|
||||
rowStart_(0), rowEnd_(height), blockStart_(0)
|
||||
{
|
||||
fillOffsets(firstBlockDim, lastBlockDim);
|
||||
assertInvariants();
|
||||
|
|
@ -182,15 +182,15 @@ namespace gtsam {
|
|||
protected:
|
||||
void assertInvariants() const {
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(blockStart_ < variableColOffsets_.size());
|
||||
assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
|
||||
assert(rowStart_ <= matrix_.rows());
|
||||
assert(rowEnd_ <= matrix_.rows());
|
||||
assert(rowStart_ <= rowEnd_);
|
||||
}
|
||||
|
||||
void checkBlock(Index block) const {
|
||||
void checkBlock(DenseIndex block) const {
|
||||
assert(matrix_.cols() == variableColOffsets_.back());
|
||||
assert(block < variableColOffsets_.size()-1);
|
||||
assert(block < (DenseIndex)variableColOffsets_.size()-1);
|
||||
assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -35,28 +35,22 @@ namespace gtsam {
|
|||
* \nosubgrouping
|
||||
*/
|
||||
template<class FACTOR, class DERIVEDCONDITIONAL>
|
||||
class ConditionalUnordered {
|
||||
|
||||
class ConditionalUnordered
|
||||
{
|
||||
protected:
|
||||
|
||||
/** The first nrFrontal variables are frontal and the rest are parents. */
|
||||
size_t nrFrontals_;
|
||||
|
||||
/** Iterator over keys */
|
||||
using typename FACTOR::iterator; // 'using' instead of typedef to avoid ambiguous symbol from multiple inheritance
|
||||
|
||||
/** Const iterator over keys */
|
||||
using typename FACTOR::const_iterator; // 'using' instead of typedef to avoid ambiguous symbol from multiple inheritance
|
||||
|
||||
public:
|
||||
|
||||
private:
|
||||
/// Typedef to this class
|
||||
typedef ConditionalUnordered<FACTOR,DERIVEDCONDITIONAL> This;
|
||||
|
||||
public:
|
||||
/** View of the frontal keys (call frontals()) */
|
||||
typedef boost::iterator_range<const_iterator> Frontals;
|
||||
typedef boost::iterator_range<typename FACTOR::const_iterator> Frontals;
|
||||
|
||||
/** View of the separator keys (call parents()) */
|
||||
typedef boost::iterator_range<const_iterator> Parents;
|
||||
typedef boost::iterator_range<typename FACTOR::const_iterator> Parents;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
|
@ -103,34 +97,32 @@ namespace gtsam {
|
|||
Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); }
|
||||
|
||||
/** Iterator pointing to first frontal key. */
|
||||
const_iterator beginFrontals() const { return asFactor().begin(); }
|
||||
typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); }
|
||||
|
||||
/** Iterator pointing past the last frontal key. */
|
||||
const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; }
|
||||
typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; }
|
||||
|
||||
/** Iterator pointing to the first parent key. */
|
||||
const_iterator beginParents() const { return endFrontals(); }
|
||||
typename FACTOR::const_iterator beginParents() const { return endFrontals(); }
|
||||
|
||||
/** Iterator pointing past the last parent key. */
|
||||
const_iterator endParents() const { return asFactor().end(); }
|
||||
typename FACTOR::const_iterator endParents() const { return asFactor().end(); }
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Mutable iterators and accessors */
|
||||
iterator beginFrontals() {
|
||||
return asFactor().begin();
|
||||
} ///<TODO: comment
|
||||
iterator endFrontals() {
|
||||
return asFactor().begin() + nrFrontals_;
|
||||
} ///<TODO: comment
|
||||
iterator beginParents() {
|
||||
return asFactor().begin() + nrFrontals_;
|
||||
} ///<TODO: comment
|
||||
iterator endParents() {
|
||||
return asFactor().end();
|
||||
} ///<TODO: comment
|
||||
/** Mutable iterator pointing to first frontal key. */
|
||||
typename FACTOR::iterator beginFrontals() { return asFactor().begin(); }
|
||||
|
||||
/** Mutable iterator pointing past the last frontal key. */
|
||||
typename FACTOR::iterator endFrontals() { return asFactor().begin() + nrFrontals_; }
|
||||
|
||||
/** Mutable iterator pointing to the first parent key. */
|
||||
typename FACTOR::iterator beginParents() { return asFactor().begin() + nrFrontals_; }
|
||||
|
||||
/** Mutable iterator pointing past the last parent key. */
|
||||
typename FACTOR::iterator endParents() { return asFactor().end(); }
|
||||
|
||||
private:
|
||||
// Cast to factor type (non-const) (casts down to derived conditional type, then up to factor type)
|
||||
|
|
|
|||
|
|
@ -31,8 +31,8 @@ namespace gtsam {
|
|||
{
|
||||
if(ordering && variableIndex) {
|
||||
// Do elimination
|
||||
std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<FACTORGRAPH> > result
|
||||
= ELIMINATIONTREE(asDerived(), *variableIndex, *ordering).eliminate(function);
|
||||
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> > result
|
||||
= EliminationTreeType(asDerived(), *variableIndex, *ordering).eliminate(function);
|
||||
// If any factors are remaining, the ordering was incomplete
|
||||
if(!result.second->empty())
|
||||
throw InconsistentEliminationRequested();
|
||||
|
|
@ -61,8 +61,8 @@ namespace gtsam {
|
|||
{
|
||||
if(ordering && variableIndex) {
|
||||
// Do elimination with given ordering
|
||||
std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<FACTORGRAPH> > result
|
||||
= JUNCTIONTREE(ELIMINATIONTREE(asDerived(), *variableIndex, *ordering)).eliminate(function);
|
||||
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > result
|
||||
= JunctionTreeType(EliminationTreeType(asDerived(), *variableIndex, *ordering)).eliminate(function);
|
||||
// If any factors are remaining, the ordering was incomplete
|
||||
if(!result.second->empty())
|
||||
throw InconsistentEliminationRequested();
|
||||
|
|
@ -87,14 +87,14 @@ namespace gtsam {
|
|||
template<class FACTORGRAPH>
|
||||
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
|
||||
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
|
||||
const Eliminate& function, const OrderingUnordered& ordering, OptionalVariableIndex variableIndex) const
|
||||
const OrderingUnordered& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(variableIndex) {
|
||||
// Do elimination
|
||||
return ELIMINATIONTREE(asDerived(), *variableIndex, ordering).eliminate(function);
|
||||
return EliminationTreeType(asDerived(), *variableIndex, ordering).eliminate(function);
|
||||
} else {
|
||||
// If no variable index is provided, compute one and call this function again
|
||||
return eliminatePartialSequential(function, ordering, VariableIndexUnordered(asDerived()));
|
||||
return eliminatePartialSequential(ordering, function, VariableIndexUnordered(asDerived()));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -102,14 +102,14 @@ namespace gtsam {
|
|||
template<class FACTORGRAPH>
|
||||
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
|
||||
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
|
||||
const Eliminate& function, const OrderingUnordered& ordering, OptionalVariableIndex variableIndex) const
|
||||
const OrderingUnordered& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
|
||||
{
|
||||
if(variableIndex) {
|
||||
// Do elimination
|
||||
return JUNCTIONTREE(ELIMINATIONTREE(asDerived(), *variableIndex, ordering)).eliminate(function);
|
||||
return JunctionTreeType(EliminationTreeType(asDerived(), *variableIndex, ordering)).eliminate(function);
|
||||
} else {
|
||||
// If no variable index is provided, compute one and call this function again
|
||||
return eliminatePartialMultifrontal(function, ordering, VariableIndexUnordered(asDerived()));
|
||||
return eliminatePartialMultifrontal(ordering, function, VariableIndexUnordered(asDerived()));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -30,10 +30,11 @@ namespace gtsam {
|
|||
/// elimination, etc. This must be defined for each factor graph that inherits from
|
||||
/// EliminateableFactorGraph.
|
||||
template<class GRAPH>
|
||||
class EliminationTraits
|
||||
struct EliminationTraits
|
||||
{
|
||||
// Template for deriving:
|
||||
// typedef MyFactor FactorType; ///< Type of factors in factor graph (e.g. GaussianFactor)
|
||||
// typedef MyFactorGraphType FactorGraphType; ///< Type of the factor graph (e.g. GaussianFactorGraph)
|
||||
// typedef MyConditional ConditionalType; ///< Type of conditionals from elimination (e.g. GaussianConditional)
|
||||
// typedef MyBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination (e.g. GaussianBayesNet)
|
||||
// typedef MyEliminationTree EliminationTreeType; ///< Type of elimination tree (e.g. GaussianEliminationTree)
|
||||
|
|
@ -41,8 +42,7 @@ namespace gtsam {
|
|||
// typedef MyJunctionTree JunctionTreeType; ///< Type of Junction tree (e.g. GaussianJunctionTree)
|
||||
// static pair<shared_ptr<ConditionalType>, shared_ptr<FactorType>
|
||||
// DefaultEliminate(
|
||||
// const std::vector<boost::shared_ptr<FactorType> >& factors,
|
||||
// const std::vector<Key>& keys); ///< The default dense elimination function
|
||||
// const MyFactorGraph& factors, const OrderingUnordered& keys); ///< The default dense elimination function
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -55,12 +55,12 @@ namespace gtsam {
|
|||
{
|
||||
private:
|
||||
typedef EliminateableFactorGraph<FACTORGRAPH> This; ///< Typedef to this class.
|
||||
typedef FACTORGRAPH FactorGraphType;
|
||||
typedef FACTORGRAPH FactorGraphType; ///< Typedef to factor graph type
|
||||
// Base factor type stored in this graph (private because derived classes will get this from
|
||||
// their FactorGraph base class)
|
||||
typedef typename EliminationTraits<FactorGraphType>::FactorType _FactorType;
|
||||
|
||||
public:
|
||||
/// Base factor type stored in this graph
|
||||
typedef typename EliminationTraits<FactorGraphType>::FactorType FactorType;
|
||||
|
||||
/// Conditional type stored in the Bayes net produced by elimination
|
||||
typedef typename EliminationTraits<FactorGraphType>::ConditionalType ConditionalType;
|
||||
|
||||
|
|
@ -78,10 +78,10 @@ namespace gtsam {
|
|||
|
||||
/// The pair of conditional and remaining factor produced by a single dense elimination step on
|
||||
/// a subgraph.
|
||||
typedef std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > EliminationResult;
|
||||
typedef std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<_FactorType> > EliminationResult;
|
||||
|
||||
/// The function type that does a single dense elimination step on a subgraph.
|
||||
typedef boost::function<EliminationResult(std::vector<boost::shared_ptr<FactorType> >, std::vector<Key>)> Eliminate;
|
||||
typedef boost::function<EliminationResult(std::vector<boost::shared_ptr<_FactorType> >, std::vector<Key>)> Eliminate;
|
||||
|
||||
/// Typedef for an optional ordering as an argument to elimination functions
|
||||
typedef boost::optional<const OrderingUnordered&> OptionalOrdering;
|
||||
|
|
|
|||
|
|
@ -137,6 +137,7 @@ namespace gtsam {
|
|||
} catch(std::invalid_argument& e) {
|
||||
// If this is thrown from structure[order[j]] above, it means that it was requested to
|
||||
// eliminate a variable not present in the graph, so throw a more informative error message.
|
||||
(void)e; // Prevent unused variable warning
|
||||
throw std::invalid_argument("EliminationTree: given ordering contains variables that are not involved in the factor graph");
|
||||
} catch(...) {
|
||||
throw;
|
||||
|
|
|
|||
|
|
@ -43,7 +43,6 @@ namespace gtsam {
|
|||
public:
|
||||
typedef FACTOR FactorType; ///< factor type
|
||||
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
|
||||
typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional; ///< Shared pointer to a conditional
|
||||
typedef typename std::vector<sharedFactor>::iterator iterator;
|
||||
typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
|
||||
|
||||
|
|
|
|||
|
|
@ -73,17 +73,17 @@ namespace gtsam {
|
|||
// current node did not introduce any parents beyond those already in the child.
|
||||
|
||||
// Do symbolic elimination for this node
|
||||
std::vector<SymbolicFactorUnordered::shared_ptr> symbolicFactors;
|
||||
SymbolicFactorGraphUnordered symbolicFactors;
|
||||
symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size());
|
||||
// Add symbolic versions of the ETree node factors
|
||||
BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, ETreeNode->factors) {
|
||||
symbolicFactors.push_back(boost::make_shared<SymbolicFactorUnordered>(
|
||||
SymbolicFactorUnordered::FromKeys(*factor))); }
|
||||
// Add symbolic factors passed up from children
|
||||
symbolicFactors.insert(symbolicFactors.end(), myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end());
|
||||
std::vector<Key> keyAsVector(1); keyAsVector[0] = ETreeNode->key;
|
||||
symbolicFactors.push_back(myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end());
|
||||
OrderingUnordered keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key);
|
||||
std::pair<SymbolicConditionalUnordered::shared_ptr, SymbolicFactorUnordered::shared_ptr> symbolicElimResult =
|
||||
EliminateSymbolicUnordered(symbolicFactors, keyAsVector);
|
||||
EliminateSymbolicUnordered(symbolicFactors, keyAsOrdering);
|
||||
|
||||
// Store symbolic elimination results in the parent
|
||||
myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first);
|
||||
|
|
|
|||
|
|
@ -38,14 +38,14 @@ namespace gtsam {
|
|||
BaseFactor(terms, d, sigmas), BaseConditional(nrFrontals) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename KEYS, class MATRIX>
|
||||
template<typename KEYS>
|
||||
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)
|
||||
GaussianConditionalUnordered::shared_ptr GaussianConditionalUnordered::Combine(ITERATOR firstConditional, ITERATOR lastConditional)
|
||||
{
|
||||
// TODO: check for being a clique
|
||||
|
||||
|
|
|
|||
|
|
@ -75,7 +75,7 @@ namespace gtsam {
|
|||
return false;
|
||||
|
||||
// check if R_ and d_ are linear independent
|
||||
for (size_t i=0; i<Ab_.rows(); i++) {
|
||||
for (DenseIndex i=0; i<Ab_.rows(); i++) {
|
||||
list<Vector> rows1; rows1.push_back(Vector(get_R().row(i)));
|
||||
list<Vector> rows2; rows2.push_back(Vector(c.get_R().row(i)));
|
||||
|
||||
|
|
|
|||
|
|
@ -81,7 +81,7 @@ namespace gtsam {
|
|||
* factor. */
|
||||
template<typename KEYS>
|
||||
GaussianConditionalUnordered(
|
||||
const KEYS& keys, size_t nrFrontals const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas);
|
||||
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
|
||||
|
|
|
|||
|
|
@ -19,6 +19,8 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraphUnordered.h>
|
||||
#include <gtsam/linear/VectorValuesUnordered.h>
|
||||
#include <gtsam/linear/GaussianBayesTreeUnordered.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/cholesky.h>
|
||||
|
|
@ -28,6 +30,12 @@ using namespace gtsam;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianFactorGraphUnordered::push_back_bayesTree(const GaussianBayesTreeUnordered& bayesTree)
|
||||
{
|
||||
Base::push_back_bayesTree(bayesTree);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraphUnordered::Keys GaussianFactorGraphUnordered::keys() const {
|
||||
FastSet<Key> keys;
|
||||
|
|
@ -141,132 +149,60 @@ namespace gtsam {
|
|||
augmented.leftCols(augmented.cols()-1),
|
||||
augmented.col(augmented.cols()-1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraphUnordered::EliminationResult EliminateJacobians(const FactorGraph<
|
||||
JacobianFactorUnordered>& factors, size_t nrFrontals) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix GaussianFactorGraphUnordered::augmentedHessian() const {
|
||||
// combine all factors and get upper-triangular part of Hessian
|
||||
HessianFactorUnordered combined(*this);
|
||||
Matrix result = combined.info();
|
||||
// Fill in lower-triangular part of Hessian
|
||||
result.triangularView<Eigen::StrictlyLower>() = result.transpose();
|
||||
return result;
|
||||
}
|
||||
//Matrix GaussianFactorGraphUnordered::augmentedHessian() const {
|
||||
// // combine all factors and get upper-triangular part of Hessian
|
||||
// HessianFactorUnordered combined(*this);
|
||||
// Matrix result = combined.info();
|
||||
// // Fill in lower-triangular part of Hessian
|
||||
// result.triangularView<Eigen::StrictlyLower>() = result.transpose();
|
||||
// return result;
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<Matrix,Vector> GaussianFactorGraphUnordered::hessian() const {
|
||||
Matrix augmented = augmentedHessian();
|
||||
return make_pair(
|
||||
augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
|
||||
augmented.col(augmented.rows()-1).head(augmented.rows()-1));
|
||||
}
|
||||
//std::pair<Matrix,Vector> GaussianFactorGraphUnordered::hessian() const {
|
||||
// Matrix augmented = augmentedHessian();
|
||||
// return make_pair(
|
||||
// augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
|
||||
// augmented.col(augmented.rows()-1).head(augmented.rows()-1));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraphUnordered::EliminationResult EliminateCholesky(const FactorGraph<
|
||||
GaussianFactorUnordered>& factors, size_t nrFrontals) {
|
||||
//GaussianFactorGraphUnordered::EliminationResult EliminateCholesky(const FactorGraph<
|
||||
// GaussianFactorUnordered>& factors, size_t nrFrontals) {
|
||||
|
||||
const bool debug = ISDEBUG("EliminateCholesky");
|
||||
// const bool debug = ISDEBUG("EliminateCholesky");
|
||||
|
||||
// Form Ab' * Ab
|
||||
gttic(combine);
|
||||
HessianFactorUnordered::shared_ptr combinedFactor(new HessianFactorUnordered(factors));
|
||||
gttoc(combine);
|
||||
// // Form Ab' * Ab
|
||||
// gttic(combine);
|
||||
// HessianFactorUnordered::shared_ptr combinedFactor(new HessianFactorUnordered(factors));
|
||||
// gttoc(combine);
|
||||
|
||||
// Do Cholesky, note that after this, the lower triangle still contains
|
||||
// some untouched non-zeros that should be zero. We zero them while
|
||||
// extracting submatrices next.
|
||||
gttic(partial_Cholesky);
|
||||
combinedFactor->partialCholesky(nrFrontals);
|
||||
// // Do Cholesky, note that after this, the lower triangle still contains
|
||||
// // some untouched non-zeros that should be zero. We zero them while
|
||||
// // extracting submatrices next.
|
||||
// gttic(partial_Cholesky);
|
||||
// combinedFactor->partialCholesky(nrFrontals);
|
||||
|
||||
gttoc(partial_Cholesky);
|
||||
// gttoc(partial_Cholesky);
|
||||
|
||||
// Extract conditional and fill in details of the remaining factor
|
||||
gttic(split);
|
||||
GaussianConditional::shared_ptr conditional =
|
||||
combinedFactor->splitEliminatedFactor(nrFrontals);
|
||||
if (debug) {
|
||||
conditional->print("Extracted conditional: ");
|
||||
combinedFactor->print("Eliminated factor (L piece): ");
|
||||
}
|
||||
gttoc(split);
|
||||
// // Extract conditional and fill in details of the remaining factor
|
||||
// gttic(split);
|
||||
// GaussianConditional::shared_ptr conditional =
|
||||
// combinedFactor->splitEliminatedFactor(nrFrontals);
|
||||
// if (debug) {
|
||||
// conditional->print("Extracted conditional: ");
|
||||
// combinedFactor->print("Eliminated factor (L piece): ");
|
||||
// }
|
||||
// gttoc(split);
|
||||
|
||||
combinedFactor->assertInvariants();
|
||||
return make_pair(conditional, combinedFactor);
|
||||
}
|
||||
// combinedFactor->assertInvariants();
|
||||
// return make_pair(conditional, combinedFactor);
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
static FactorGraph<JacobianFactorUnordered> convertToJacobians(const FactorGraph<
|
||||
GaussianFactorUnordered>& factors) {
|
||||
|
||||
typedef JacobianFactorUnordered J;
|
||||
typedef HessianFactorUnordered H;
|
||||
|
||||
const bool debug = ISDEBUG("convertToJacobians");
|
||||
|
||||
FactorGraph<J> jacobians;
|
||||
jacobians.reserve(factors.size());
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, factors)
|
||||
if (factor) {
|
||||
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
|
||||
if (jacobian) {
|
||||
jacobians.push_back(jacobian);
|
||||
if (debug) jacobian->print("Existing JacobianFactor: ");
|
||||
} else {
|
||||
H::shared_ptr hessian(boost::dynamic_pointer_cast<H>(factor));
|
||||
if (!hessian) throw std::invalid_argument(
|
||||
"convertToJacobians: factor is neither a JacobianFactor nor a HessianFactor.");
|
||||
J::shared_ptr converted(new J(*hessian));
|
||||
if (debug) {
|
||||
cout << "Converted HessianFactor to JacobianFactor:\n";
|
||||
hessian->print("HessianFactor: ");
|
||||
converted->print("JacobianFactor: ");
|
||||
if (!assert_equal(*hessian, HessianFactorUnordered(*converted), 1e-3)) throw runtime_error(
|
||||
"convertToJacobians: Conversion between Jacobian and Hessian incorrect");
|
||||
}
|
||||
jacobians.push_back(converted);
|
||||
}
|
||||
}
|
||||
return jacobians;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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");
|
||||
|
||||
// Convert all factors to the appropriate type and call the type-specific EliminateGaussian.
|
||||
if (debug) cout << "Using QR" << endl;
|
||||
|
||||
gttic(convert_to_Jacobian);
|
||||
FactorGraph<JacobianFactorUnordered> jacobians = convertToJacobians(factors);
|
||||
gttoc(convert_to_Jacobian);
|
||||
|
||||
gttic(Jacobian_EliminateGaussian);
|
||||
GaussianConditional::shared_ptr conditional;
|
||||
GaussianFactorUnordered::shared_ptr factor;
|
||||
boost::tie(conditional, factor) = EliminateJacobians(jacobians, nrFrontals);
|
||||
gttoc(Jacobian_EliminateGaussian);
|
||||
|
||||
return make_pair(conditional, factor);
|
||||
} // \EliminateQR
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool hasConstraints(const FactorGraph<GaussianFactorUnordered>& factors) {
|
||||
bool hasConstraints(const GaussianFactorGraphUnordered& factors) {
|
||||
typedef JacobianFactorUnordered J;
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, factors) {
|
||||
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
|
||||
|
|
@ -278,38 +214,40 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraphUnordered::EliminationResult EliminatePreferCholesky(
|
||||
const FactorGraph<GaussianFactorUnordered>& 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
|
||||
// to HessianFactors. This is because QR can handle constrained noise
|
||||
// models but Cholesky cannot.
|
||||
if (hasConstraints(factors))
|
||||
return EliminateQR(factors, nrFrontals);
|
||||
else {
|
||||
GaussianFactorGraphUnordered::EliminationResult ret;
|
||||
gttic(EliminateCholesky);
|
||||
ret = EliminateCholesky(factors, nrFrontals);
|
||||
gttoc(EliminateCholesky);
|
||||
return ret;
|
||||
}
|
||||
// // If any JacobianFactors have constrained noise models, we have to convert
|
||||
// // all factors to JacobianFactors. Otherwise, we can convert all factors
|
||||
// // to HessianFactors. This is because QR can handle constrained noise
|
||||
// // models but Cholesky cannot.
|
||||
// if (hasConstraints(factors))
|
||||
// return EliminateQR(factors, nrFrontals);
|
||||
// else {
|
||||
// GaussianFactorGraphUnordered::EliminationResult ret;
|
||||
// gttic(EliminateCholesky);
|
||||
// ret = EliminateCholesky(factors, nrFrontals);
|
||||
// gttoc(EliminateCholesky);
|
||||
// return ret;
|
||||
// }
|
||||
|
||||
} // \EliminatePreferCholesky
|
||||
//} // \EliminatePreferCholesky
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
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<JacobianFactorUnordered>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
|
||||
namespace {
|
||||
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<JacobianFactorUnordered>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
|
||||
}
|
||||
return result;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Errors operator*(const GaussianFactorGraphUnordered& fg, const VectorValues& x) {
|
||||
Errors operator*(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x) {
|
||||
Errors e;
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
|
|
@ -319,12 +257,12 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValues& x, Errors& e) {
|
||||
void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x, Errors& e) {
|
||||
multiplyInPlace(fg,x,e.begin());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValues& x, const Errors::iterator& e) {
|
||||
void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x, const Errors::iterator& e) {
|
||||
Errors::iterator ei = e;
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
|
|
@ -335,7 +273,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// x += alpha*A'*e
|
||||
void transposeMultiplyAdd(const GaussianFactorGraphUnordered& fg, double alpha, const Errors& e, VectorValues& x) {
|
||||
void transposeMultiplyAdd(const GaussianFactorGraphUnordered& fg, double alpha, const Errors& e, VectorValuesUnordered& x) {
|
||||
// For each factor add the gradient contribution
|
||||
Errors::const_iterator ei = e.begin();
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
|
|
@ -345,9 +283,9 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues gradient(const GaussianFactorGraphUnordered& fg, const VectorValues& x0) {
|
||||
VectorValuesUnordered gradient(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x0) {
|
||||
// It is crucial for performance to make a zero-valued clone of x
|
||||
VectorValues g = VectorValues::Zero(x0);
|
||||
VectorValuesUnordered g = VectorValuesUnordered::Zero(x0);
|
||||
Errors e;
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
|
|
@ -358,7 +296,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void gradientAtZero(const GaussianFactorGraphUnordered& fg, VectorValues& g) {
|
||||
void gradientAtZero(const GaussianFactorGraphUnordered& fg, VectorValuesUnordered& g) {
|
||||
// Zero-out the gradient
|
||||
g.setZero();
|
||||
Errors e;
|
||||
|
|
@ -370,20 +308,20 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void residual(const GaussianFactorGraphUnordered& fg, const VectorValues &x, VectorValues &r) {
|
||||
void residual(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r) {
|
||||
Key i = 0 ;
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
r[i] = Ai->getb();
|
||||
i++;
|
||||
}
|
||||
VectorValues Ax = VectorValues::SameStructure(r);
|
||||
VectorValuesUnordered Ax = VectorValuesUnordered::SameStructure(r);
|
||||
multiply(fg,x,Ax);
|
||||
axpy(-1.0,Ax,r);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void multiply(const GaussianFactorGraphUnordered& fg, const VectorValues &x, VectorValues &r) {
|
||||
void multiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r) {
|
||||
r.setZero();
|
||||
Key i = 0;
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
|
|
@ -397,7 +335,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void transposeMultiply(const GaussianFactorGraphUnordered& fg, const VectorValues &r, VectorValues &x) {
|
||||
void transposeMultiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &r, VectorValuesUnordered &x) {
|
||||
x.setZero();
|
||||
Key i = 0;
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
|
|
@ -410,7 +348,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraphUnordered& fg, const VectorValues& x) {
|
||||
boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x) {
|
||||
boost::shared_ptr<Errors> e(new Errors);
|
||||
BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
|
||||
JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@
|
|||
#include <gtsam/inference/EliminateableFactorGraph.h>
|
||||
#include <gtsam/linear/GaussianFactorUnordered.h>
|
||||
#include <gtsam/linear/JacobianFactorUnordered.h>
|
||||
#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -37,16 +38,11 @@ namespace gtsam {
|
|||
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>
|
||||
template<> struct EliminationTraits<GaussianFactorGraphUnordered>
|
||||
{
|
||||
typedef GaussianFactorUnordered FactorType; ///< Type of factors in factor graph
|
||||
typedef GaussianFactorGraphUnordered FactorGraphType; ///< Type of the factor graph (e.g. GaussianFactorGraph)
|
||||
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
|
||||
|
|
@ -54,8 +50,8 @@ namespace gtsam {
|
|||
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); }
|
||||
DefaultEliminate(const FactorGraphType& factors, const OrderingUnordered& keys) {
|
||||
return EliminateQRUnordered(factors, keys); }
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -120,6 +116,9 @@ namespace gtsam {
|
|||
void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model) {
|
||||
add(JacobianFactorUnordered(terms,b,model)); }
|
||||
|
||||
/** push back a BayesTree as a collection of factors. */
|
||||
void push_back_bayesTree(const GaussianBayesTreeUnordered& bayesTree);
|
||||
|
||||
/**
|
||||
* Return the set of variables involved in the factors (computes a set
|
||||
* union).
|
||||
|
|
@ -191,7 +190,7 @@ namespace gtsam {
|
|||
and the negative log-likelihood is
|
||||
\f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
|
||||
*/
|
||||
Matrix augmentedHessian() const;
|
||||
//Matrix augmentedHessian() const;
|
||||
|
||||
/**
|
||||
* Return the dense Hessian \f$ \Lambda \f$ and information vector
|
||||
|
|
@ -199,7 +198,7 @@ namespace gtsam {
|
|||
* is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also
|
||||
* GaussianFactorGraph::augmentedHessian.
|
||||
*/
|
||||
std::pair<Matrix,Vector> hessian() const;
|
||||
//std::pair<Matrix,Vector> hessian() const;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
@ -211,48 +210,11 @@ namespace gtsam {
|
|||
|
||||
};
|
||||
|
||||
/**
|
||||
* Combine and eliminate several factors.
|
||||
* \addtogroup LinearSolving
|
||||
*/
|
||||
GTSAM_EXPORT JacobianFactor::shared_ptr CombineJacobians(
|
||||
const FactorGraph<JacobianFactor>& factors,
|
||||
const VariableSlots& variableSlots);
|
||||
|
||||
/**
|
||||
* Evaluates whether linear factors have any constrained noise models
|
||||
* @return true if any factor is constrained.
|
||||
*/
|
||||
GTSAM_EXPORT bool hasConstraints(const FactorGraph<GaussianFactor>& factors);
|
||||
|
||||
/**
|
||||
* Densely combine and partially eliminate JacobianFactors to produce a
|
||||
* single conditional with nrFrontals frontal variables and a remaining
|
||||
* factor.
|
||||
* Variables are eliminated in the natural order of the variable indices of in the factors.
|
||||
* @param factors Factors to combine and eliminate
|
||||
* @param nrFrontals Number of frontal variables to eliminate.
|
||||
* @return The conditional and remaining factor
|
||||
|
||||
* \addtogroup LinearSolving
|
||||
*/
|
||||
GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph<
|
||||
JacobianFactor>& factors, size_t nrFrontals = 1);
|
||||
|
||||
/**
|
||||
* Densely partially eliminate with QR factorization. HessianFactors are
|
||||
* first factored with Cholesky decomposition to produce JacobianFactors,
|
||||
* by calling the conversion constructor JacobianFactor(const HessianFactor&).
|
||||
* Variables are eliminated in the natural order of the variable indices of in
|
||||
* the factors.
|
||||
* @param factors Factors to combine and eliminate
|
||||
* @param nrFrontals Number of frontal variables to eliminate.
|
||||
* @return The conditional and remaining factor
|
||||
|
||||
* \addtogroup LinearSolving
|
||||
*/
|
||||
GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph<
|
||||
GaussianFactor>& factors, size_t nrFrontals = 1);
|
||||
GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraphUnordered& factors);
|
||||
|
||||
/**
|
||||
* Densely partially eliminate with Cholesky factorization. JacobianFactors
|
||||
|
|
@ -273,8 +235,8 @@ namespace gtsam {
|
|||
|
||||
* \addtogroup LinearSolving
|
||||
*/
|
||||
GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminatePreferCholesky(const FactorGraph<
|
||||
GaussianFactor>& factors, size_t nrFrontals = 1);
|
||||
//GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminatePreferCholesky(const FactorGraph<
|
||||
// GaussianFactor>& factors, size_t nrFrontals = 1);
|
||||
|
||||
/**
|
||||
* Densely partially eliminate with Cholesky factorization. JacobianFactors
|
||||
|
|
@ -294,22 +256,22 @@ namespace gtsam {
|
|||
|
||||
* \addtogroup LinearSolving
|
||||
*/
|
||||
GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
|
||||
GaussianFactor>& factors, size_t nrFrontals = 1);
|
||||
//GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
|
||||
// GaussianFactor>& factors, size_t nrFrontals = 1);
|
||||
|
||||
/****** Linear Algebra Opeations ******/
|
||||
|
||||
/** return A*x */
|
||||
GTSAM_EXPORT Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x);
|
||||
GTSAM_EXPORT Errors operator*(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x);
|
||||
|
||||
/** In-place version e <- A*x that overwrites e. */
|
||||
GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e);
|
||||
GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x, Errors& e);
|
||||
|
||||
/** In-place version e <- A*x that takes an iterator. */
|
||||
GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e);
|
||||
GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x, const Errors::iterator& e);
|
||||
|
||||
/** x += alpha*A'*e */
|
||||
GTSAM_EXPORT void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x);
|
||||
GTSAM_EXPORT void transposeMultiplyAdd(const GaussianFactorGraphUnordered& fg, double alpha, const Errors& e, VectorValuesUnordered& x);
|
||||
|
||||
/**
|
||||
* Compute the gradient of the energy function,
|
||||
|
|
@ -318,9 +280,10 @@ namespace gtsam {
|
|||
* The gradient is \f$ A^T(Ax-b) \f$.
|
||||
* @param fg The Jacobian factor graph $(A,b)$
|
||||
* @param x0 The center about which to compute the gradient
|
||||
* @return The gradient as a VectorValues
|
||||
* @return The gradient as a VectorValuesUnordered
|
||||
|
||||
*/
|
||||
GTSAM_EXPORT VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0);
|
||||
GTSAM_EXPORT VectorValuesUnordered gradient(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x0);
|
||||
|
||||
/**
|
||||
* Compute the gradient of the energy function,
|
||||
|
|
@ -328,23 +291,19 @@ namespace gtsam {
|
|||
* centered around zero.
|
||||
* The gradient is \f$ A^T(Ax-b) \f$.
|
||||
* @param fg The Jacobian factor graph $(A,b)$
|
||||
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
|
||||
* @return The gradient as a VectorValues
|
||||
* @param [output] g A VectorValuesUnordered to store the gradient, which must be preallocated, see allocateVectorValues
|
||||
* @return The gradient as a VectorValuesUnordered
|
||||
|
||||
*/
|
||||
GTSAM_EXPORT void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g);
|
||||
GTSAM_EXPORT void gradientAtZero(const GaussianFactorGraphUnordered& fg, VectorValuesUnordered& g);
|
||||
|
||||
/* matrix-vector operations */
|
||||
GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
|
||||
GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
|
||||
GTSAM_EXPORT void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x);
|
||||
|
||||
/** shared pointer version
|
||||
* \todo Make this a member function - affects SubgraphPreconditioner */
|
||||
GTSAM_EXPORT boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x);
|
||||
GTSAM_EXPORT void residual(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r);
|
||||
GTSAM_EXPORT void multiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r);
|
||||
GTSAM_EXPORT void transposeMultiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &r, VectorValuesUnordered &x);
|
||||
|
||||
/** return A*x-b
|
||||
* \todo Make this a member function - affects SubgraphPreconditioner */
|
||||
inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) {
|
||||
return *gaussianErrors_(fg, x); }
|
||||
GTSAM_EXPORT Errors gaussianErrors(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@
|
|||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class VectorValues;
|
||||
class VectorValuesUnordered;
|
||||
|
||||
/**
|
||||
* An abstract virtual base class for JacobianFactor and HessianFactor. A GaussianFactor has a
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
#include <boost/range/adaptor/transformed.hpp>
|
||||
#include <boost/range/join.hpp>
|
||||
#include <boost/assign/list_of.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -33,13 +34,13 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename KEYS, class MATRIX>
|
||||
template<typename KEYS>
|
||||
JacobianFactorUnordered::JacobianFactorUnordered(
|
||||
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas) :
|
||||
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) :
|
||||
Base(keys)
|
||||
{
|
||||
// Check noise model dimension
|
||||
if(noiseModel && model->dim() != augmentedMatrix.rows())
|
||||
if(model && model->dim() != augmentedMatrix.rows())
|
||||
throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim());
|
||||
|
||||
// Check number of variables
|
||||
|
|
@ -62,13 +63,20 @@ namespace gtsam {
|
|||
model_ = model;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace {
|
||||
const Matrix& _getPairSecond(const std::pair<Key,Matrix>& p) {
|
||||
return p.second;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename TERMS>
|
||||
void JacobianFactorUnordered::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel)
|
||||
{
|
||||
// Check noise model dimension
|
||||
if(noiseModel && model->dim() != b.size())
|
||||
throw InvalidNoiseModel(b.size(), model->dim());
|
||||
if(noiseModel && noiseModel->dim() != b.size())
|
||||
throw InvalidNoiseModel(b.size(), noiseModel->dim());
|
||||
|
||||
// Resize base class key vector
|
||||
Base::keys_.resize(terms.size());
|
||||
|
|
@ -79,10 +87,14 @@ namespace gtsam {
|
|||
using boost::adaptors::map_values;
|
||||
using boost::adaptors::transformed;
|
||||
using boost::join;
|
||||
Ab_ = VerticalBlockMatrix(join(terms | map_values | transformed(Matrix::cols), cref_list_of<1>(1)), b.size());
|
||||
Ab_ = VerticalBlockMatrix(join(
|
||||
terms
|
||||
| transformed(&_getPairSecond)
|
||||
| transformed(boost::mem_fn(&Matrix::cols)),
|
||||
cref_list_of<1>((DenseIndex)1)), b.size());
|
||||
|
||||
// Check and add terms
|
||||
typedef pair<Key, Matrix> Term;
|
||||
typedef std::pair<Key, Matrix> Term;
|
||||
DenseIndex i = 0; // For block index
|
||||
BOOST_FOREACH(const Term& term, terms) {
|
||||
// Check block rows
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
#include <gtsam/linear/GaussianFactorGraphUnordered.h>
|
||||
#include <gtsam/linear/VectorValuesUnordered.h>
|
||||
#include <gtsam/inference/VariableSlots.h>
|
||||
#include <gtsam/inference/OrderingUnordered.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
|
@ -44,6 +45,7 @@
|
|||
#pragma GCC diagnostic pop
|
||||
#endif
|
||||
#include <boost/range/algorithm/copy.hpp>
|
||||
#include <boost/range/adaptor/indirected.hpp>
|
||||
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
|
|
@ -63,7 +65,6 @@ namespace gtsam {
|
|||
// *this = JacobianFactorUnordered(*rhs);
|
||||
else
|
||||
throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
|
||||
assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -77,11 +78,11 @@ namespace gtsam {
|
|||
JacobianFactorUnordered::JacobianFactorUnordered(Key i1, const Matrix& A1,
|
||||
const Vector& b, const SharedDiagonal& model)
|
||||
{
|
||||
fillTerms(cref_list_of<1>(make_pair(i1,A1)), b, model);
|
||||
fillTerms(cref_list_of<1>(make_pair(i1, A1)), b, model);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactorUnordered::JacobianFactorUnordered(Key i1, const Matrix& A1, Key i2, const Matrix& A2,
|
||||
JacobianFactorUnordered::JacobianFactorUnordered(const Key i1, const Matrix& A1, Key i2, const Matrix& A2,
|
||||
const Vector& b, const SharedDiagonal& model)
|
||||
{
|
||||
fillTerms(cref_list_of<2>
|
||||
|
|
@ -90,7 +91,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactorUnordered::JacobianFactorUnordered(Key i1, const Matrix& A1, Key i2, const Matrix& A2,
|
||||
JacobianFactorUnordered::JacobianFactorUnordered(const Key i1, const Matrix& A1, Key i2, const Matrix& A2,
|
||||
Key i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model)
|
||||
{
|
||||
fillTerms(cref_list_of<3>
|
||||
|
|
@ -126,28 +127,28 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
// 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)
|
||||
boost::tuple<vector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
|
||||
const std::vector<JacobianFactorUnordered::shared_ptr>& factors, const vector<VariableSlots::const_iterator>& variableSlots)
|
||||
{
|
||||
gttic(countDims);
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
|
||||
vector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
|
||||
#else
|
||||
vector<size_t> varDims(variableSlots.size());
|
||||
vector<DenseIndex> varDims(variableSlots.size());
|
||||
#endif
|
||||
size_t m = 0;
|
||||
size_t n = 0;
|
||||
DenseIndex m = 0;
|
||||
DenseIndex n = 0;
|
||||
{
|
||||
size_t jointVarpos = 0;
|
||||
BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) {
|
||||
|
||||
assert(slots.second.size() == factors.size());
|
||||
BOOST_FOREACH(VariableSlots::const_iterator slots, variableSlots)
|
||||
{
|
||||
assert(slots->second.size() == factors.size());
|
||||
|
||||
size_t sourceFactorI = 0;
|
||||
BOOST_FOREACH(const size_t sourceVarpos, slots.second) {
|
||||
BOOST_FOREACH(const size_t sourceVarpos, slots->second) {
|
||||
if(sourceVarpos < numeric_limits<size_t>::max()) {
|
||||
const JacobianFactorUnordered& sourceFactor = *factors[sourceFactorI];
|
||||
size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
|
||||
DenseIndex vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
|
||||
varDims[jointVarpos] = vardim;
|
||||
|
|
@ -192,7 +193,9 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactorUnordered::JacobianFactorUnordered(
|
||||
const GaussianFactorGraphUnordered& factors, boost::optional<const VariableSlots&> variableSlots)
|
||||
const GaussianFactorGraphUnordered& graph,
|
||||
boost::optional<const OrderingUnordered&> ordering,
|
||||
boost::optional<const VariableSlots&> variableSlots)
|
||||
{
|
||||
gttic(JacobianFactorUnordered_combine_constructor);
|
||||
|
||||
|
|
@ -200,36 +203,70 @@ namespace gtsam {
|
|||
gttic(Compute_VariableSlots);
|
||||
boost::optional<VariableSlots> computedVariableSlots;
|
||||
if(!variableSlots) {
|
||||
computedVariableSlots = VariableSlots(factors);
|
||||
computedVariableSlots = VariableSlots(graph);
|
||||
variableSlots = computedVariableSlots; // Binds reference, does not copy VariableSlots
|
||||
}
|
||||
gttoc(Compute_VariableSlots);
|
||||
|
||||
// Cast or convert to Jacobians
|
||||
std::vector<JacobianFactorUnordered::shared_ptr> jacobians = _convertOrCastToJacobians(factors);
|
||||
std::vector<JacobianFactorUnordered::shared_ptr> jacobians = _convertOrCastToJacobians(graph);
|
||||
|
||||
// Order variable slots
|
||||
vector<VariableSlots::const_iterator> orderedSlots;
|
||||
orderedSlots.reserve(variableSlots->size());
|
||||
if(ordering) {
|
||||
// If an ordering is provided, arrange the slots first that ordering
|
||||
FastList<VariableSlots::const_iterator> unorderedSlots;
|
||||
size_t nOrderingSlotsUsed = 0;
|
||||
orderedSlots.resize(ordering->size());
|
||||
FastMap<Key, size_t> inverseOrdering = ordering->invert();
|
||||
for(VariableSlots::const_iterator item = variableSlots->begin(); item != variableSlots->end(); ++item) {
|
||||
FastMap<Key, size_t>::const_iterator orderingPosition = inverseOrdering.find(item->first);
|
||||
if(orderingPosition == inverseOrdering.end()) {
|
||||
unorderedSlots.push_back(item);
|
||||
} else {
|
||||
orderedSlots[orderingPosition->second] = item;
|
||||
++ nOrderingSlotsUsed;
|
||||
}
|
||||
}
|
||||
if(nOrderingSlotsUsed != ordering->size())
|
||||
throw std::invalid_argument(
|
||||
"The ordering provided to the JacobianFactor combine constructor\n"
|
||||
"contained extra variables that did not appear in the factors to combine.");
|
||||
// Add the remaining slots
|
||||
BOOST_FOREACH(VariableSlots::const_iterator item, unorderedSlots) {
|
||||
orderedSlots.push_back(item);
|
||||
}
|
||||
} else {
|
||||
// If no ordering is provided, arrange the slots as they were, which will be sorted
|
||||
// numerically since VariableSlots uses a map sorting on Key.
|
||||
for(VariableSlots::const_iterator item = variableSlots->begin(); item != variableSlots->end(); ++item)
|
||||
orderedSlots.push_back(item);
|
||||
}
|
||||
|
||||
// Count dimensions
|
||||
vector<size_t> varDims;
|
||||
size_t m, n;
|
||||
boost::tie(varDims, m, n) = _countDims(jacobians, *variableSlots);
|
||||
vector<DenseIndex> varDims;
|
||||
DenseIndex m, n;
|
||||
boost::tie(varDims, m, n) = _countDims(jacobians, orderedSlots);
|
||||
|
||||
// Allocate matrix and copy keys in order
|
||||
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
|
||||
Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1>((DenseIndex)1)), m); // Allocate augmented matrix
|
||||
Base::keys_.resize(orderedSlots.size());
|
||||
boost::range::copy(orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin()); // Get variable keys
|
||||
gttoc(allocate);
|
||||
|
||||
// Loop over slots in combined factor and copy blocks from source factors
|
||||
gttic(copy_blocks);
|
||||
// Loop over slots in combined factor
|
||||
size_t combinedSlot = 0;
|
||||
BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) {
|
||||
BOOST_FOREACH(VariableSlots::const_iterator varslot, orderedSlots) {
|
||||
JacobianFactorUnordered::ABlock destSlot(this->getA(this->begin()+combinedSlot));
|
||||
// Loop over source jacobians
|
||||
size_t nextRow = 0;
|
||||
DenseIndex nextRow = 0;
|
||||
for(size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
|
||||
// Slot in source factor
|
||||
const size_t sourceSlot = varslot.second[factorI];
|
||||
const size_t sourceRows = jacobians[factorI]->rows();
|
||||
const size_t sourceSlot = varslot->second[factorI];
|
||||
const DenseIndex 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<size_t>::max())
|
||||
|
|
@ -242,13 +279,14 @@ namespace gtsam {
|
|||
}
|
||||
gttoc(copy_blocks);
|
||||
|
||||
// Copy the RHS vectors and sigmas
|
||||
gttic(copy_vectors);
|
||||
bool anyConstrained = false;
|
||||
boost::optional<Vector> sigmas;
|
||||
// Loop over source jacobians
|
||||
size_t nextRow = 0;
|
||||
DenseIndex nextRow = 0;
|
||||
for(size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
|
||||
const size_t sourceRows = jacobians[factorI]->rows();
|
||||
const DenseIndex 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.
|
||||
|
|
@ -421,8 +459,53 @@ namespace gtsam {
|
|||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianConditionalUnordered::shared_ptr JacobianFactorUnordered::eliminateFirst() {
|
||||
return this->eliminate(1);
|
||||
std::pair<boost::shared_ptr<GaussianConditionalUnordered>, boost::shared_ptr<JacobianFactorUnordered> >
|
||||
JacobianFactorUnordered::eliminate(const OrderingUnordered& keys)
|
||||
{
|
||||
GaussianFactorGraphUnordered graph;
|
||||
graph.add(*this);
|
||||
return EliminateQRUnordered(graph, keys);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void JacobianFactorUnordered::setModel(bool anyConstrained, const Vector& sigmas) {
|
||||
if((size_t) sigmas.size() != this->rows())
|
||||
throw InvalidNoiseModel(this->rows(), sigmas.size());
|
||||
if (anyConstrained)
|
||||
model_ = noiseModel::Constrained::MixedSigmas(sigmas);
|
||||
else
|
||||
model_ = noiseModel::Diagonal::Sigmas(sigmas);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<boost::shared_ptr<GaussianConditionalUnordered>, boost::shared_ptr<JacobianFactorUnordered> >
|
||||
EliminateQRUnordered(const GaussianFactorGraphUnordered& factors, const OrderingUnordered& keys)
|
||||
{
|
||||
// Combine and sort variable blocks in elimination order
|
||||
JacobianFactorUnordered::shared_ptr jointFactor;
|
||||
try {
|
||||
jointFactor = boost::make_shared<JacobianFactorUnordered>(factors, keys);
|
||||
} catch(std::invalid_argument& e) {
|
||||
(void) e; // Avoid unused variable warning
|
||||
throw InvalidDenseElimination(
|
||||
"EliminateQRUnordered was called with a request to eliminate variables that are not\n"
|
||||
"involved in the provided factors.");
|
||||
}
|
||||
|
||||
// Do dense elimination
|
||||
SharedDiagonal noiseModel;
|
||||
if(jointFactor->model_)
|
||||
jointFactor->model_ = jointFactor->model_->QR(jointFactor->Ab_.matrix());
|
||||
else
|
||||
inplace_QR(jointFactor->Ab_.matrix());
|
||||
|
||||
// Zero below the diagonal
|
||||
jointFactor->Ab_.matrix().triangularView<Eigen::StrictlyLower>().setZero();
|
||||
|
||||
// Split elimination result into conditional and remaining factor
|
||||
GaussianConditionalUnordered::shared_ptr conditional = jointFactor->splitConditional(keys.size());
|
||||
|
||||
return make_pair(conditional, jointFactor);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -441,9 +524,12 @@ namespace gtsam {
|
|||
gttic(cond_Rd);
|
||||
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());
|
||||
GaussianConditionalUnordered::shared_ptr conditional(boost::make_shared<GaussianConditionalUnordered>(
|
||||
Base::keys_, nrFrontals, Ab_, sigmas));
|
||||
SharedDiagonal conditionalNoiseModel;
|
||||
if(model_)
|
||||
conditionalNoiseModel =
|
||||
noiseModel::Diagonal::Sigmas(model_->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart()));
|
||||
GaussianConditionalUnordered::shared_ptr conditional = boost::make_shared<GaussianConditionalUnordered>(
|
||||
Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
|
||||
Ab_.rowStart() += frontalDim;
|
||||
Ab_.firstBlock() += nrFrontals;
|
||||
Ab_.rowEnd() = originalRowEnd;
|
||||
|
|
@ -464,63 +550,4 @@ namespace gtsam {
|
|||
return conditional;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianConditionalUnordered::shared_ptr JacobianFactorUnordered::eliminate(size_t 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");
|
||||
|
||||
if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl;
|
||||
if(debug) this->print("Eliminating JacobianFactor: ");
|
||||
if(debug) gtsam::print(matrix_, "Augmented Ab: ");
|
||||
|
||||
size_t frontalDim = Ab_.range(0,nrFrontals).cols();
|
||||
|
||||
if(debug) cout << "frontalDim = " << frontalDim << endl;
|
||||
|
||||
// Use in-place QR dense Ab appropriate to NoiseModel
|
||||
gttic(QR);
|
||||
SharedDiagonal noiseModel = model_->QR(matrix_);
|
||||
gttoc(QR);
|
||||
|
||||
// Zero the lower-left triangle. todo: not all of these entries actually
|
||||
// need to be zeroed if we are careful to start copying rows after the last
|
||||
// structural zero.
|
||||
if(matrix_.rows() > 0)
|
||||
for(size_t j=0; j<(size_t) matrix_.cols(); ++j)
|
||||
for(size_t i=j+1; i<noiseModel->dim(); ++i)
|
||||
matrix_(i,j) = 0.0;
|
||||
|
||||
if(debug) gtsam::print(matrix_, "QR result: ");
|
||||
if(debug) noiseModel->print("QR result noise model: ");
|
||||
|
||||
// Start of next part
|
||||
model_ = noiseModel;
|
||||
return splitConditional(nrFrontals);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//void JacobianFactorUnordered::allocate(const VariableSlots& variableSlots, vector<
|
||||
// size_t>& varDims, size_t m) {
|
||||
// keys_.resize(variableSlots.size());
|
||||
// std::transform(variableSlots.begin(), variableSlots.end(), begin(),
|
||||
// boost::bind(&VariableSlots::const_iterator::value_type::first, _1));
|
||||
// varDims.push_back(1);
|
||||
// Ab_.copyStructureFrom(BlockAb(matrix_, varDims.begin(), varDims.end(), m));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void JacobianFactorUnordered::setModel(bool anyConstrained, const Vector& sigmas) {
|
||||
if((size_t) sigmas.size() != this->rows())
|
||||
throw InvalidNoiseModel(this->rows(), sigmas.size());
|
||||
if (anyConstrained)
|
||||
model_ = noiseModel::Constrained::MixedSigmas(sigmas);
|
||||
else
|
||||
model_ = noiseModel::Diagonal::Sigmas(sigmas);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@ namespace gtsam {
|
|||
class GaussianFactorGraphUnordered;
|
||||
class GaussianConditionalUnordered;
|
||||
class VectorValuesUnordered;
|
||||
class OrderingUnordered;
|
||||
|
||||
/**
|
||||
* A Gaussian factor in the squared-error form.
|
||||
|
|
@ -136,6 +137,7 @@ namespace gtsam {
|
|||
* computation performed. */
|
||||
explicit JacobianFactorUnordered(
|
||||
const GaussianFactorGraphUnordered& graph,
|
||||
boost::optional<const OrderingUnordered&> ordering = boost::none,
|
||||
boost::optional<const VariableSlots&> variableSlots = boost::none);
|
||||
|
||||
/** Virtual destructor */
|
||||
|
|
@ -260,42 +262,42 @@ namespace gtsam {
|
|||
std::vector<boost::tuple<size_t, size_t, double> >
|
||||
sparse(const std::vector<size_t>& columnIndices) const;
|
||||
|
||||
/**
|
||||
* Return a whitened version of the factor, i.e. with unit diagonal noise
|
||||
* model. */
|
||||
/** Return a whitened version of the factor, i.e. with unit diagonal noise model. */
|
||||
JacobianFactorUnordered whiten() const;
|
||||
|
||||
/** Eliminate the first variable, modifying the factor in place to contain the remaining marginal. */
|
||||
boost::shared_ptr<GaussianConditionalUnordered> eliminateFirst();
|
||||
|
||||
/** Eliminate the requested number of frontal variables, modifying the factor in place to contain the remaining marginal. */
|
||||
boost::shared_ptr<GaussianConditionalUnordered> eliminate(size_t nrFrontals = 1);
|
||||
|
||||
/**
|
||||
* splits a pre-factorized factor into a conditional, and changes the current
|
||||
* factor to be the remaining component. Performs same operation as eliminate(),
|
||||
* but without running QR.
|
||||
*/
|
||||
boost::shared_ptr<GaussianConditionalUnordered> splitConditional(size_t nrFrontals = 1);
|
||||
|
||||
// following methods all used in CombineJacobians:
|
||||
// Many imperative, perhaps all need to be combined in constructor
|
||||
|
||||
/** allocate space */
|
||||
//void allocate(const VariableSlots& variableSlots,
|
||||
// std::vector<size_t>& varDims, size_t m);
|
||||
/** Eliminate the requested variables. */
|
||||
std::pair<boost::shared_ptr<GaussianConditionalUnordered>, boost::shared_ptr<JacobianFactorUnordered> >
|
||||
eliminate(const OrderingUnordered& keys);
|
||||
|
||||
/** set noiseModel correctly */
|
||||
void setModel(bool anyConstrained, const Vector& sigmas);
|
||||
|
||||
/** Assert invariants after elimination */
|
||||
void assertInvariants() const;
|
||||
|
||||
/**
|
||||
* Densely partially eliminate with QR factorization, this is usually provided as an argument to
|
||||
* one of the factor graph elimination functions (see EliminateableFactorGraph). HessianFactors
|
||||
* are first factored with Cholesky decomposition to produce JacobianFactors, by calling the
|
||||
* conversion constructor JacobianFactor(const HessianFactor&). Variables are eliminated in the
|
||||
* order specified in \c keys.
|
||||
* @param factors Factors to combine and eliminate
|
||||
* @param keys The variables to eliminate in the order as specified here in \c keys
|
||||
* @return The conditional and remaining factor
|
||||
*
|
||||
* \addtogroup LinearSolving */
|
||||
friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditionalUnordered>, boost::shared_ptr<JacobianFactorUnordered> >
|
||||
EliminateQRUnordered(const GaussianFactorGraphUnordered& factors, const OrderingUnordered& keys);
|
||||
|
||||
private:
|
||||
|
||||
/// Internal function to fill blocks and set dimensions
|
||||
template<typename TERMS>
|
||||
void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
|
||||
|
||||
/**
|
||||
* splits a pre-factorized factor into a conditional, and changes the current
|
||||
* factor to be the remaining component. Performs same operation as eliminate(),
|
||||
* but without running QR.
|
||||
*/
|
||||
boost::shared_ptr<GaussianConditionalUnordered> splitConditional(size_t nrFrontals = 1);
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
|
|||
|
|
@ -131,33 +131,6 @@ bool VectorValuesUnordered::hasSameStructure(const VectorValuesUnordered& other)
|
|||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VectorValuesUnordered::permuteInPlace(const Permutation& permutation) {
|
||||
// Allocate new values
|
||||
Values newValues(this->size());
|
||||
|
||||
// Swap values from this VectorValues to the permuted VectorValues
|
||||
for(size_t i = 0; i < this->size(); ++i)
|
||||
newValues[i].swap(this->at(permutation[i]));
|
||||
|
||||
// Swap the values into this VectorValues
|
||||
values_.swap(newValues);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VectorValues::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
|
||||
if(selector.size() != permutation.size())
|
||||
throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
|
||||
// Create new index the size of the permuted entries
|
||||
Values reorderedEntries(selector.size());
|
||||
// Permute the affected entries into the new index
|
||||
for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
|
||||
reorderedEntries[dstSlot].swap(values_[selector[permutation[dstSlot]]]);
|
||||
// Put the affected entries back in the new order
|
||||
for(size_t slot = 0; slot < selector.size(); ++slot)
|
||||
values_[selector[slot]].swap(reorderedEntries[slot]);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void VectorValuesUnordered::swap(VectorValuesUnordered& other) {
|
||||
this->values_.swap(other.values_);
|
||||
|
|
|
|||
|
|
@ -181,7 +181,7 @@ namespace gtsam {
|
|||
|
||||
/** Named constructor to create a VectorValues that matches the structure of
|
||||
* the specified VectorValues, but do not initialize the new values. */
|
||||
static VectorValuesUnordered SameStructure(const VectorValues& other);
|
||||
static VectorValuesUnordered SameStructure(const VectorValuesUnordered& other);
|
||||
|
||||
/** Named constructor to create a VectorValues from a container of variable
|
||||
* dimensions that is filled with zeros.
|
||||
|
|
@ -318,7 +318,7 @@ namespace gtsam {
|
|||
* scale a vector by a scalar
|
||||
*/
|
||||
friend VectorValuesUnordered operator*(const double a, const VectorValuesUnordered &v) {
|
||||
VectorValues result = VectorValues::SameStructure(v);
|
||||
VectorValuesUnordered result = VectorValuesUnordered::SameStructure(v);
|
||||
for(Index j = 0; j < v.size(); ++j)
|
||||
result.values_[j] = a * v.values_[j];
|
||||
return result;
|
||||
|
|
@ -415,7 +415,7 @@ namespace gtsam {
|
|||
// in the first and last iterators, and concatenates them in that order into the
|
||||
// output.
|
||||
template<typename ITERATOR>
|
||||
const VectorValuesUnordered extractVectorValuesSlices(const VectorValuesUnordered& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) {
|
||||
const Vector extractVectorValuesSlices(const VectorValuesUnordered& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) {
|
||||
// Find total dimensionality
|
||||
size_t dim = 0;
|
||||
for(ITERATOR j = first; j != last; ++j)
|
||||
|
|
|
|||
|
|
@ -141,7 +141,7 @@ on gtsam::IndeterminantLinearSystemException for more information.\n";
|
|||
const DenseIndex blockRows; ///< The dimensionality of the noise model
|
||||
|
||||
InvalidMatrixBlock(DenseIndex factorRows, DenseIndex blockRows) :
|
||||
factorRows(factorRows), blockRows(noiseModelDims) {}
|
||||
factorRows(factorRows), blockRows(blockRows) {}
|
||||
virtual ~InvalidMatrixBlock() throw() {}
|
||||
|
||||
virtual const char* what() const throw();
|
||||
|
|
@ -150,4 +150,10 @@ on gtsam::IndeterminantLinearSystemException for more information.\n";
|
|||
mutable std::string description_;
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
class InvalidDenseElimination : public std::invalid_argument {
|
||||
public:
|
||||
InvalidDenseElimination(const char *message) : std::invalid_argument(message) {}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -35,9 +35,10 @@ namespace gtsam {
|
|||
class SymbolicJunctionTreeUnordered;
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<> class EliminationTraits<SymbolicFactorGraphUnordered>
|
||||
template<> struct EliminationTraits<SymbolicFactorGraphUnordered>
|
||||
{
|
||||
typedef SymbolicFactorUnordered FactorType; ///< Type of factors in factor graph
|
||||
typedef SymbolicFactorGraphUnordered FactorGraphType; ///< Type of the factor graph (e.g. GaussianFactorGraph)
|
||||
typedef SymbolicConditionalUnordered ConditionalType; ///< Type of conditionals from elimination
|
||||
typedef SymbolicBayesNetUnordered BayesNetType; ///< Type of Bayes net from sequential elimination
|
||||
typedef SymbolicEliminationTreeUnordered EliminationTreeType; ///< Type of elimination tree
|
||||
|
|
@ -45,8 +46,8 @@ namespace gtsam {
|
|||
typedef SymbolicJunctionTreeUnordered 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 EliminateSymbolicUnordered(factors, keys); }
|
||||
DefaultEliminate(const FactorGraphType& factors, const OrderingUnordered& keys) {
|
||||
return EliminateSymbolicUnordered(factors, keys); }
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -18,8 +18,10 @@
|
|||
#include <boost/foreach.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
#include <gtsam/inference/OrderingUnordered.h>
|
||||
#include <gtsam/symbolic/SymbolicFactorUnordered.h>
|
||||
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
|
||||
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -27,7 +29,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
std::pair<boost::shared_ptr<SymbolicConditionalUnordered>, boost::shared_ptr<SymbolicFactorUnordered> >
|
||||
EliminateSymbolicUnordered(const vector<SymbolicFactorUnordered::shared_ptr>& factors, const vector<Key>& keys)
|
||||
EliminateSymbolicUnordered(const SymbolicFactorGraphUnordered& factors, const OrderingUnordered& keys)
|
||||
{
|
||||
// Gather all keys
|
||||
FastSet<Key> allKeys;
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declaration of SymbolicConditional
|
||||
// Forward declarations
|
||||
class SymbolicConditionalUnordered;
|
||||
|
||||
/**
|
||||
|
|
@ -114,11 +114,15 @@ namespace gtsam {
|
|||
}
|
||||
}; // IndexFactor
|
||||
|
||||
// Forward declarations
|
||||
class SymbolicFactorGraphUnordered;
|
||||
class OrderingUnordered;
|
||||
|
||||
/** Dense elimination function for symbolic factors. This is usually provided as an argument to
|
||||
* one of the factor graph elimination functions (see EliminateableFactorGraph). The factor
|
||||
* graph elimination functions do sparse variable elimination, and use this function to eliminate
|
||||
* single variables or variable cliques. */
|
||||
GTSAM_EXPORT std::pair<boost::shared_ptr<SymbolicConditionalUnordered>, boost::shared_ptr<SymbolicFactorUnordered> >
|
||||
EliminateSymbolicUnordered(const std::vector<SymbolicFactorUnordered::shared_ptr>& factors, const std::vector<Key>& keys);
|
||||
EliminateSymbolicUnordered(const SymbolicFactorGraphUnordered& factors, const OrderingUnordered& keys);
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue