115 lines
3.9 KiB
C++
115 lines
3.9 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file inference-inl.h
|
|
* @brief inference template definitions
|
|
* @author Frank Dellaert, Richard Roberts
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <limits>
|
|
#include <map>
|
|
#include <stdexcept>
|
|
|
|
#include <boost/foreach.hpp>
|
|
#include <boost/format.hpp>
|
|
#include <boost/lambda/bind.hpp>
|
|
#include <boost/lambda/lambda.hpp>
|
|
#include <boost/pool/pool_alloc.hpp>
|
|
|
|
#include <ccolamd.h>
|
|
|
|
#include <gtsam/base/timing.h>
|
|
#include <gtsam/inference/inference.h>
|
|
#include <gtsam/inference/FactorGraph-inl.h>
|
|
#include <gtsam/inference/BayesNet-inl.h>
|
|
#include <gtsam/inference/ConditionalBase.h>
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
template<class VARIABLEINDEXTYPE, typename CONSTRAINTCONTAINER>
|
|
Permutation::shared_ptr Inference::PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex, const CONSTRAINTCONTAINER& constrainLast) {
|
|
size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
|
|
// Convert to compressed column major format colamd wants it in (== MATLAB format!)
|
|
int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */
|
|
int * A = new int[Alen]; /* colamd arg 4: row indices of A, of size Alen */
|
|
int * p = new int[nVars + 1]; /* colamd arg 5: column pointers of A, of size n_col+1 */
|
|
int * cmember = new int[nVars]; /* Constraint set of A, of size n_col */
|
|
|
|
static const bool debug = false;
|
|
|
|
p[0] = 0;
|
|
int count = 0;
|
|
for(Index var = 0; var < variableIndex.size(); ++var) {
|
|
const typename VARIABLEINDEXTYPE::Factors& column(variableIndex[var]);
|
|
size_t lastFactorId = numeric_limits<size_t>::max();
|
|
BOOST_FOREACH(const size_t& factorIndex, column) {
|
|
if(lastFactorId != numeric_limits<size_t>::max())
|
|
assert(factorIndex > lastFactorId);
|
|
A[count++] = factorIndex; // copy sparse column
|
|
if(debug) cout << "A[" << count-1 << "] = " << factorIndex << endl;
|
|
}
|
|
p[var+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1
|
|
cmember[var] = 0;
|
|
}
|
|
|
|
// If at least some variables are not constrained to be last, constrain the
|
|
// ones that should be constrained.
|
|
if(constrainLast.size() < variableIndex.size()) {
|
|
BOOST_FOREACH(Index var, constrainLast) {
|
|
assert(var < nVars);
|
|
cmember[var] = 1;
|
|
}
|
|
}
|
|
|
|
assert((size_t)count == variableIndex.nEntries());
|
|
|
|
if(debug)
|
|
for(size_t i=0; i<nVars+1; ++i)
|
|
cout << "p[" << i << "] = " << p[i] << endl;
|
|
|
|
double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
|
|
// double knobs[CCOLAMD_KNOBS];
|
|
// ccolamd_set_defaults(knobs);
|
|
// knobs[CCOLAMD_DENSE_ROW]=-1;
|
|
// knobs[CCOLAMD_DENSE_COL]=-1;
|
|
|
|
int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
|
|
|
|
// call colamd, result will be in p
|
|
/* returns (1) if successful, (0) otherwise*/
|
|
int rv = ccolamd(nFactors, nVars, Alen, A, p, knobs, stats, cmember);
|
|
if(rv != 1)
|
|
throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str());
|
|
delete[] A; // delete symbolic A
|
|
delete[] cmember;
|
|
|
|
// ccolamd_report(stats);
|
|
|
|
// Convert elimination ordering in p to an ordering
|
|
Permutation::shared_ptr permutation(new Permutation(nVars));
|
|
for (Index j = 0; j < nVars; j++) {
|
|
permutation->operator[](j) = p[j];
|
|
if(debug) cout << "COLAMD: " << j << "->" << p[j] << endl;
|
|
}
|
|
if(debug) cout << "COLAMD: p[" << nVars << "] = " << p[nVars] << endl;
|
|
delete[] p; // delete colamd result vector
|
|
|
|
return permutation;
|
|
}
|
|
|
|
} // namespace gtsam
|