12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

forceOrder flag respects order of constrained variables passed to ccolamd

release/4.3a0
Frank Dellaert 2012-09-17 00:24:49 +00:00
parent 33b772fc21
commit db8264aaae
2 changed files with 19 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam/inference/inference-inl.h
View File

@ -32,31 +32,39 @@ namespace inference {
/* ************************************************************************* */
template<typename CONSTRAINED>
Permutation::shared_ptr PermutationCOLAMD(
const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
const VariableIndex& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder) {
std::vector<int> cmember(variableIndex.size(), 0);
size_t n = variableIndex.size();
std::vector<int> cmember(n, 0);
// If at least some variables are not constrained to be last, constrain the
// ones that should be constrained.
if(constrainLast.size() < variableIndex.size()) {
if(constrainLast.size() < n) {
BOOST_FOREACH(Index var, constrainLast) {
assert(var < variableIndex.size());
assert(var < n);
cmember[var] = 1;
}
}
return PermutationCOLAMD_(variableIndex, cmember);
Permutation::shared_ptr permutation = PermutationCOLAMD_(variableIndex, cmember);
if (forceOrder) {
Index j=n;
BOOST_REVERSE_FOREACH(Index c, constrainLast)
permutation->operator[](--j) = c;
}
return permutation;
}
/* ************************************************************************* */
template<typename CONSTRAINED_MAP>
Permutation::shared_ptr PermutationCOLAMDGrouped(
const VariableIndex& variableIndex, const CONSTRAINED_MAP& constraints) {
std::vector<int> cmember(variableIndex.size(), 0);
size_t n = variableIndex.size();
std::vector<int> cmember(n, 0);
typedef typename CONSTRAINED_MAP::value_type constraint_pair;
BOOST_FOREACH(const constraint_pair& p, constraints) {
assert(p.first < variableIndex.size());
assert(p.first < n);
// FIXME: check that no groups are skipped
cmember[p.first] = p.second;
}
@ -66,7 +74,8 @@ Permutation::shared_ptr PermutationCOLAMDGrouped(
/* ************************************************************************* */
inline Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex) {
std::vector<int> cmember(variableIndex.size(), 0);
size_t n = variableIndex.size();
std::vector<int> cmember(n, 0);
return PermutationCOLAMD_(variableIndex, cmember);
}

3
gtsam/inference/inference.h
View File

@ -43,10 +43,11 @@ namespace gtsam {
* @param variableIndex is the variable index lookup from a graph
* @param constrainlast is a vector of keys that should be constrained
* @tparam constrainLast is a std::vector (or similar structure)
* @param forceOrder if true, will not allow re-ordering of constrained variables
*/
template<typename CONSTRAINED>
Permutation::shared_ptr PermutationCOLAMD(
const VariableIndex& variableIndex, const CONSTRAINED& constrainLast);
const VariableIndex& variableIndex, const CONSTRAINED& constrainLast, bool forceOrder=false);
/**
* Compute a permutation of variable ordering using constrained colamd to