Ordering.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #include <vector>
20 #include <limits>
21 
22 #include <boost/format.hpp>
23 
26 
27 #ifdef GTSAM_SUPPORT_NESTED_DISSECTION
29 #endif
30 
31 using namespace std;
32 
33 namespace gtsam {
34 
35 /* ************************************************************************* */
36 FastMap<Key, size_t> Ordering::invert() const {
37  FastMap<Key, size_t> inverted;
38  for (size_t pos = 0; pos < this->size(); ++pos)
39  inverted.insert(make_pair((*this)[pos], pos));
40  return inverted;
41 }
42 
43 /* ************************************************************************* */
44 Ordering Ordering::Colamd(const VariableIndex& variableIndex) {
45  // Call constrained version with all groups set to zero
46  vector<int> dummy_groups(variableIndex.size(), 0);
47  return Ordering::ColamdConstrained(variableIndex, dummy_groups);
48 }
49 
50 /* ************************************************************************* */
51 Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
52  std::vector<int>& cmember) {
53  gttic(Ordering_COLAMDConstrained);
54 
55  gttic(Prepare);
56  const size_t nVars = variableIndex.size();
57  if (nVars == 0)
58  {
59  return Ordering();
60  }
61 
62  if (nVars == 1)
63  {
64  return Ordering(KeyVector(1, variableIndex.begin()->first));
65  }
66 
67  const size_t nEntries = variableIndex.nEntries(), nFactors =
68  variableIndex.nFactors();
69  // Convert to compressed column major format colamd wants it in (== MATLAB format!)
70  const size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors,
71  (int) nVars); /* colamd arg 3: size of the array A */
72  vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
73  vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
74 
75  // Fill in input data for COLAMD
76  p[0] = 0;
77  int count = 0;
78  KeyVector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
79  size_t index = 0;
80  for (auto key_factors: variableIndex) {
81  // Arrange factor indices into COLAMD format
82  const FactorIndices& column = key_factors.second;
83  for(size_t factorIndex: column) {
84  A[count++] = (int) factorIndex; // copy sparse column
85  }
86  p[index + 1] = count; // column j (base 1) goes from A[j-1] to A[j]-1
87  // Store key in array and increment index
88  keys[index] = key_factors.first;
89  ++index;
90  }
91 
92  assert((size_t)count == variableIndex.nEntries());
93 
94  //double* knobs = nullptr; /* colamd arg 6: parameters (uses defaults if nullptr) */
95  double knobs[CCOLAMD_KNOBS];
96  ccolamd_set_defaults(knobs);
97  knobs[CCOLAMD_DENSE_ROW] = -1;
98  knobs[CCOLAMD_DENSE_COL] = -1;
99 
100  int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
101 
102  gttoc(Prepare);
103 
104  // call colamd, result will be in p
105  /* returns (1) if successful, (0) otherwise*/
106  if (nVars > 0) {
107  gttic(ccolamd);
108  int rv = ccolamd((int) nFactors, (int) nVars, (int) Alen, &A[0], &p[0],
109  knobs, stats, &cmember[0]);
110  if (rv != 1)
111  throw runtime_error(
112  (boost::format("ccolamd failed with return value %1%") % rv).str());
113  }
114 
115  // ccolamd_report(stats);
116 
117  // Convert elimination ordering in p to an ordering
118  gttic(Fill_Ordering);
120  result.resize(nVars);
121  for (size_t j = 0; j < nVars; ++j)
122  result[j] = keys[p[j]];
123  gttoc(Fill_Ordering);
124 
125  return result;
126 }
127 
128 /* ************************************************************************* */
129 Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
130  const KeyVector& constrainLast, bool forceOrder) {
131  gttic(Ordering_COLAMDConstrainedLast);
132 
133  size_t n = variableIndex.size();
134  std::vector<int> cmember(n, 0);
135 
136  // Build a mapping to look up sorted Key indices by Key
137  // TODO(frank): think of a way to not build this
138  FastMap<Key, size_t> keyIndices;
139  size_t j = 0;
140  for (auto key_factors: variableIndex)
141  keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));
142 
143  // If at least some variables are not constrained to be last, constrain the
144  // ones that should be constrained.
145  int group = (constrainLast.size() != n ? 1 : 0);
146  for (Key key: constrainLast) {
147  cmember[keyIndices.at(key)] = group;
148  if (forceOrder)
149  ++group;
150  }
151 
152  return Ordering::ColamdConstrained(variableIndex, cmember);
153 }
154 
155 /* ************************************************************************* */
156 Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex,
157  const KeyVector& constrainFirst, bool forceOrder) {
158  gttic(Ordering_COLAMDConstrainedFirst);
159 
160  const int none = -1;
161  size_t n = variableIndex.size();
162  std::vector<int> cmember(n, none);
163 
164  // Build a mapping to look up sorted Key indices by Key
165  FastMap<Key, size_t> keyIndices;
166  size_t j = 0;
167  for (auto key_factors: variableIndex)
168  keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));
169 
170  // If at least some variables are not constrained to be last, constrain the
171  // ones that should be constrained.
172  int group = 0;
173  for (Key key: constrainFirst) {
174  cmember[keyIndices.at(key)] = group;
175  if (forceOrder)
176  ++group;
177  }
178 
179  if (!forceOrder && !constrainFirst.empty())
180  ++group;
181  for(int& c: cmember)
182  if (c == none)
183  c = group;
184 
185  return Ordering::ColamdConstrained(variableIndex, cmember);
186 }
187 
188 /* ************************************************************************* */
189 Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
190  const FastMap<Key, int>& groups) {
191  gttic(Ordering_COLAMDConstrained);
192  size_t n = variableIndex.size();
193  std::vector<int> cmember(n, 0);
194 
195  // Build a mapping to look up sorted Key indices by Key
196  FastMap<Key, size_t> keyIndices;
197  size_t j = 0;
198  for (auto key_factors: variableIndex)
199  keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++));
200 
201  // Assign groups
202  typedef FastMap<Key, int>::value_type key_group;
203  for(const key_group& p: groups) {
204  // FIXME: check that no groups are skipped
205  cmember[keyIndices.at(p.first)] = p.second;
206  }
207 
208  return Ordering::ColamdConstrained(variableIndex, cmember);
209 }
210 
211 /* ************************************************************************* */
212 Ordering Ordering::Metis(const MetisIndex& met) {
213 #ifdef GTSAM_SUPPORT_NESTED_DISSECTION
214  gttic(Ordering_METIS);
215 
216  idx_t size = met.nValues();
217  if (size == 0)
218  {
219  return Ordering();
220  }
221 
222  if (size == 1)
223  {
224  return Ordering(KeyVector(1, met.intToKey(0)));
225  }
226 
227  vector<idx_t> xadj = met.xadj();
228  vector<idx_t> adj = met.adj();
229  vector<idx_t> perm, iperm;
230 
231  for (idx_t i = 0; i < size; i++) {
232  perm.push_back(0);
233  iperm.push_back(0);
234  }
235 
236  int outputError;
237 
238  outputError = METIS_NodeND(&size, &xadj[0], &adj[0], nullptr, nullptr, &perm[0],
239  &iperm[0]);
241 
242  if (outputError != METIS_OK) {
243  std::cout << "METIS failed during Nested Dissection ordering!\n";
244  return result;
245  }
246 
247  result.resize(size);
248  for (size_t j = 0; j < (size_t) size; ++j) {
249  // We have to add the minKey value back to obtain the original key in the Values
250  result[j] = met.intToKey(perm[j]);
251  }
252  return result;
253 #else
254  throw runtime_error("GTSAM was built without support for Metis-based "
255  "nested dissection");
256 #endif
257 }
258 
259 /* ************************************************************************* */
260 void Ordering::print(const std::string& str,
261  const KeyFormatter& keyFormatter) const {
262  cout << str;
263  // Print ordering in index order
264  // Print the ordering with varsPerLine ordering entries printed on each line,
265  // for compactness.
266  static const size_t varsPerLine = 10;
267  bool endedOnNewline = false;
268  for (size_t i = 0; i < size(); ++i) {
269  if (i % varsPerLine == 0)
270  cout << "Position " << i << ": ";
271  if (i % varsPerLine != 0)
272  cout << ", ";
273  cout << keyFormatter(at(i));
274  if (i % varsPerLine == varsPerLine - 1) {
275  cout << "\n";
276  endedOnNewline = true;
277  } else {
278  endedOnNewline = false;
279  }
280  }
281  if (!endedOnNewline)
282  cout << "\n";
283  cout.flush();
284 }
285 
286 /* ************************************************************************* */
287 bool Ordering::equals(const Ordering& other, double tol) const {
288  return (*this) == other;
289 }
290 
291 }
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
size_t ccolamd_recommended(int nnz, int n_row, int n_col)
return int(ret)+1
const_iterator begin() const
Iterator to the first variable entry.
void ccolamd_set_defaults(double knobs[CCOLAMD_KNOBS])
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
#define CCOLAMD_DENSE_ROW
Definition: ccolamd.h:63
idx_t idx_t * xadj
int n
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
Definition: Half.h:150
idx_t idx_t idx_t idx_t idx_t idx_t * iperm
Key intToKey(int32_t value) const
Definition: MetisIndex.h:93
bool stats
size_t size() const
The number of variable entries. This is equal to the number of unique variable Keys.
Definition: VariableIndex.h:80
int METIS_NodeND(idx_t *nvtxs, idx_t *xadj, idx_t *adjncy, idx_t *vwgt, idx_t *options, idx_t *perm, idx_t *iperm)
Definition: ometis.c:43
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
#define gttic(label)
Definition: timing.h:280
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
Values result
Definition: pytypes.h:928
size_t nValues() const
Definition: MetisIndex.h:90
int32_t idx_t
const std::vector< int32_t > & xadj() const
Definition: MetisIndex.h:84
size_t nEntries() const
The number of nonzero blocks, i.e. the number of variable-factor entries.
Definition: VariableIndex.h:86
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
idx_t idx_t idx_t idx_t idx_t * perm
#define CCOLAMD_DENSE_COL
Definition: ccolamd.h:66
traits
Definition: chartTesting.h:28
#define gttoc(label)
Definition: timing.h:281
int ccolamd(int n_row, int n_col, int Alen, int A[], int p[], double knobs[CCOLAMD_KNOBS], int stats[CCOLAMD_STATS], int cmember[])
float * p
#define CCOLAMD_KNOBS
Definition: ccolamd.h:57
size_t nFactors() const
The number of factors in the original factor graph.
Definition: VariableIndex.h:83
const G double tol
Definition: Group.h:83
const KeyVector keys
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Definition: base/Matrix.h:214
#define CCOLAMD_STATS
Definition: ccolamd.h:60
const std::vector< int32_t > & adj() const
Definition: MetisIndex.h:87
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:43:06