/* ************************************************************************* */ 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; }
/* ************************************************************************* */ 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); }
/* ************************************************************************* */ 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; }