/* ************************************************************************* */
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) {
// PRODUCT: multiply all factors
gttic(product);
DecisionTreeFactor product;
BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors)
product = (*factor) * product;
gttoc(product);
// sum out frontals, this is the factor on the separator
gttic(sum);
DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys);
gttoc(sum);
// Ordering keys for the conditional so that frontalKeys are really in front
Ordering orderedKeys;
orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end());
orderedKeys.insert(orderedKeys.end(), sum->keys().begin(), sum->keys().end());
// now divide product/sum to get conditional
gttic(divide);
DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum, orderedKeys));
gttoc(divide);
return std::make_pair(cond, sum);
}
/* ************************************************************************* */
VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial(
DenseIndex nFrontals) {
// Do dense elimination
if (blockStart() != 0)
throw std::invalid_argument(
"Can only do Cholesky when the SymmetricBlockMatrix is not a restricted view, i.e. when blockStart == 0.");
if (!gtsam::choleskyPartial(matrix_, offset(nFrontals)))
throw CholeskyFailed();
// Split conditional
// Create one big conditionals with many frontal variables.
gttic(Construct_conditional);
const size_t varDim = offset(nFrontals);
VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim);
Ab.full() = matrix_.topRows(varDim);
Ab.full().triangularView<Eigen::StrictlyLower>().setZero();
gttoc(Construct_conditional);
gttic(Remaining_factor);
// Take lower-right block of Ab_ to get the remaining factor
blockStart() = nFrontals;
gttoc(Remaining_factor);
return Ab;
}
/* ************************************************************************* */
Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex,
const std::vector<Key>& constrainFirst, bool forceOrder) {
gttic(Ordering_COLAMDConstrainedFirst);
const int none = -1;
size_t n = variableIndex.size();
std::vector<int> cmember(n, none);
// Build a mapping to look up sorted Key indices by Key
FastMap<Key, size_t> keyIndices;
size_t j = 0;
for (auto key_factors: variableIndex)
keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));
// If at least some variables are not constrained to be last, constrain the
// ones that should be constrained.
int group = 0;
for (Key key: constrainFirst) {
cmember[keyIndices.at(key)] = group;
if (forceOrder)
++group;
}
if (!forceOrder && !constrainFirst.empty())
++group;
for(int& c: cmember)
if (c == none)
c = group;
return Ordering::ColamdConstrained(variableIndex, cmember);
}
/* ************************************************************************* */
Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
const std::vector<Key>& constrainLast, bool forceOrder) {
gttic(Ordering_COLAMDConstrainedLast);
size_t n = variableIndex.size();
std::vector<int> cmember(n, 0);
// Build a mapping to look up sorted Key indices by Key
// TODO(frank): think of a way to not build this
FastMap<Key, size_t> keyIndices;
size_t j = 0;
for (auto key_factors: variableIndex)
keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));
// If at least some variables are not constrained to be last, constrain the
// ones that should be constrained.
int group = (constrainLast.size() != n ? 1 : 0);
for (Key key: constrainLast) {
cmember[keyIndices.at(key)] = group;
if (forceOrder)
++group;
}
return Ordering::ColamdConstrained(variableIndex, cmember);
}
/* ************************************************************************* */
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
EliminateDiscrete(const FactorGraph<DiscreteFactor>& factors, size_t num) {
// PRODUCT: multiply all factors
gttic(product);
DecisionTreeFactor product;
BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors)
product = (*factor) * product;
gttoc(product);
// sum out frontals, this is the factor on the separator
gttic(sum);
DecisionTreeFactor::shared_ptr sum = product.sum(num);
gttoc(sum);
// now divide product/sum to get conditional
gttic(divide);
DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum));
gttoc(divide);
return std::make_pair(cond, sum);
}
/* ************************************************************************* */
FixedLagSmoother::Result BatchFixedLagSmoother::update(
const NonlinearFactorGraph& newFactors, const Values& newTheta,
const KeyTimestampMap& timestamps) {
const bool debug = ISDEBUG("BatchFixedLagSmoother update");
if (debug) {
std::cout << "BatchFixedLagSmoother::update() START" << std::endl;
}
// Update all of the internal variables with the new information
gttic(augment_system);
// Add the new variables to theta
theta_.insert(newTheta);
// Add new variables to the end of the ordering
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
ordering_.push_back(key_value.key);
}
/* ************************************************************************* */
Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
const FastMap<Key, int>& groups) {
gttic(Ordering_COLAMDConstrained);
size_t n = variableIndex.size();
std::vector<int> cmember(n, 0);
// Build a mapping to look up sorted Key indices by Key
FastMap<Key, size_t> keyIndices;
size_t j = 0;
for (auto key_factors: variableIndex)
keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));
// Assign groups
typedef FastMap<Key, int>::value_type key_group;
for(const key_group& p: groups) {
// FIXME: check that no groups are skipped
cmember[keyIndices.at(p.first)] = p.second;
}
return Ordering::ColamdConstrained(variableIndex, cmember);
}
/* ************************************************************************* */
Ordering Ordering::Metis(const MetisIndex& met) {
#ifdef GTSAM_SUPPORT_NESTED_DISSECTION
gttic(Ordering_METIS);
vector<idx_t> xadj = met.xadj();
vector<idx_t> adj = met.adj();
vector<idx_t> perm, iperm;
idx_t size = met.nValues();
for (idx_t i = 0; i < size; i++) {
perm.push_back(0);
iperm.push_back(0);
}
int outputError;
outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0],
&iperm[0]);
Ordering result;
if (outputError != METIS_OK) {
std::cout << "METIS failed during Nested Dissection ordering!\n";
return result;
}
result.resize(size);
for (size_t j = 0; j < (size_t) size; ++j) {
// We have to add the minKey value back to obtain the original key in the Values
result[j] = met.intToKey(perm[j]);
}
return result;
#else
throw runtime_error("GTSAM was built without support for Metis-based "
"nested dissection");
#endif
}
/* ************************************************************************* */
Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
std::vector<int>& cmember) {
gttic(Ordering_COLAMDConstrained);
gttic(Prepare);
const size_t nEntries = variableIndex.nEntries(), nFactors =
variableIndex.nFactors(), nVars = variableIndex.size();
// Convert to compressed column major format colamd wants it in (== MATLAB format!)
const size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors,
(int) nVars); /* colamd arg 3: size of the array A */
vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
// Fill in input data for COLAMD
p[0] = 0;
int count = 0;
vector<Key> keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
size_t index = 0;
for (auto key_factors: variableIndex) {
// Arrange factor indices into COLAMD format
const VariableIndex::Factors& column = key_factors.second;
for(size_t factorIndex: column) {
A[count++] = (int) factorIndex; // copy sparse column
}
p[index + 1] = count; // column j (base 1) goes from A[j-1] to A[j]-1
// Store key in array and increment index
keys[index] = key_factors.first;
++index;
}
assert((size_t)count == variableIndex.nEntries());
//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 */
gttoc(Prepare);
// call colamd, result will be in p
/* returns (1) if successful, (0) otherwise*/
if (nVars > 0) {
gttic(ccolamd);
int rv = ccolamd((int) nFactors, (int) nVars, (int) Alen, &A[0], &p[0],
knobs, stats, &cmember[0]);
if (rv != 1)
throw runtime_error(
(boost::format("ccolamd failed with return value %1%") % rv).str());
}
// ccolamd_report(stats);
// Convert elimination ordering in p to an ordering
gttic(Fill_Ordering);
Ordering result;
result.resize(nVars);
for (size_t j = 0; j < nVars; ++j)
result[j] = keys[p[j]];
gttoc(Fill_Ordering);
return result;
}
/* ************************************************************************* */
DiscreteFactor::sharedValues DiscreteFactorGraph::optimize() const
{
gttic(DiscreteFactorGraph_optimize);
return BaseEliminateable::eliminateSequential()->optimize();
}
