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

Added Ordering::COLAMDConstrainedFirst function

release/4.3a0
Richard Roberts 2013-08-06 16:10:08 +00:00
parent 5692fb5e98
commit 8bd19e5ef8
3 changed files with 67 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
gtsam/inference/Ordering.cpp
View File

@ -137,6 +137,40 @@ namespace gtsam {
return Ordering::COLAMDConstrained(variableIndex, cmember);
}
/* ************************************************************************* */
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;
BOOST_FOREACH(const VariableIndex::value_type 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;
BOOST_FOREACH(Key key, constrainFirst) {
cmember[keyIndices.at(key)] = group;
if(forceOrder)
++ group;
}
if(!forceOrder)
++ group;
BOOST_FOREACH(int& c, cmember)
if(c == none)
c = group;
return Ordering::COLAMDConstrained(variableIndex, cmember);
}
/* ************************************************************************* */
Ordering Ordering::COLAMDConstrained(const VariableIndex& variableIndex,
const FastMap<Key, int>& groups)
@ -171,9 +205,11 @@ namespace gtsam {
static const size_t varsPerLine = 10;
bool endedOnNewline = false;
for(size_t i = 0; i < size(); ++i) {
if(i % varsPerLine == 0)
cout << "Position " << i << ": ";
if(i % varsPerLine != 0)
cout << ", ";
cout << i << ":" << keyFormatter(at(i));
cout << keyFormatter(at(i));
if(i % varsPerLine == varsPerLine - 1) {
cout << "\n";
endedOnNewline = true;

27
gtsam/inference/Ordering.h
View File

@ -72,7 +72,7 @@ namespace gtsam {
/// the variables in \c constrainLast to the end of the ordering, and orders all other variables
/// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c
/// constrainLast will be ordered in the same order specified in the vector<Key> \c
/// constrainLast. If \c constrainLast is false, the variables in \c constrainLast will be
/// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
template<class FACTOR>
static Ordering COLAMDConstrainedLast(const FactorGraph<FACTOR>& graph,
@ -83,11 +83,34 @@ namespace gtsam {
/// function constrains the variables in \c constrainLast to the end of the ordering, and orders
/// all other variables before in a fill-reducing ordering. If \c forceOrder is true, the
/// variables in \c constrainLast will be ordered in the same order specified in the vector<Key>
/// \c constrainLast. If \c constrainLast is false, the variables in \c constrainLast will be
/// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
static GTSAM_EXPORT Ordering COLAMDConstrainedLast(const VariableIndex& variableIndex,
const std::vector<Key>& constrainLast, bool forceOrder = false);
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
/// for note on performance). This internally builds a VariableIndex so if you already have a
/// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains
/// the variables in \c constrainLast to the end of the ordering, and orders all other variables
/// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c
/// constrainLast will be ordered in the same order specified in the vector<Key> \c
/// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
template<class FACTOR>
static Ordering COLAMDConstrainedFirst(const FactorGraph<FACTOR>& graph,
const std::vector<Key>& constrainFirst, bool forceOrder = false) {
return COLAMDConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); }
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This
/// function constrains the variables in \c constrainFirst to the front of the ordering, and
/// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the
/// variables in \c constrainFirst will be ordered in the same order specified in the
/// vector<Key> \c constrainFirst. If \c forceOrder is false, the variables in \c
/// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to
/// reduce fill-in as well.
static GTSAM_EXPORT Ordering COLAMDConstrainedFirst(const VariableIndex& variableIndex,
const std::vector<Key>& constrainFirst, bool forceOrder = false);
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
/// for note on performance). This internally builds a VariableIndex so if you already have a
/// VariableIndex, it is faster to use COLAMD(const VariableIndex&). In this function, a group

5
gtsam/inference/tests/testOrdering.cpp
View File

@ -46,6 +46,11 @@ TEST(Ordering, constrained_ordering) {
Ordering actConstrained = Ordering::COLAMDConstrainedLast(sfg, list_of(2)(4));
Ordering expConstrained = Ordering(list_of(0)(1)(5)(3)(4)(2));
EXPECT(assert_equal(expConstrained, actConstrained));
// constrained version - push one set to the start
Ordering actConstrained2 = Ordering::COLAMDConstrainedFirst(sfg, list_of(2)(4));
Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5));
EXPECT(assert_equal(expConstrained2, actConstrained2));
}
/* ************************************************************************* */