gtsam 4.1.1
gtsam
Ordering.h
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
21#pragma once
22
23#include <gtsam/inference/Key.h>
26#include <gtsam/base/FastSet.h>
27
28#include <boost/assign/list_inserter.hpp>
29#include <algorithm>
30#include <vector>
31
32namespace gtsam {
33
34class Ordering: public KeyVector {
35protected:
36 typedef KeyVector Base;
37
38public:
39
42 COLAMD, METIS, NATURAL, CUSTOM
43 };
44
45 typedef Ordering This;
46 typedef boost::shared_ptr<This> shared_ptr;
47
49 GTSAM_EXPORT
51 }
52
54 template<typename KEYS>
55 explicit Ordering(const KEYS& keys) :
56 Base(keys.begin(), keys.end()) {
57 }
58
60 template<typename ITERATOR>
61 Ordering(ITERATOR firstKey, ITERATOR lastKey) :
62 Base(firstKey, lastKey) {
63 }
64
67 boost::assign::list_inserter<boost::assign_detail::call_push_back<This> > operator+=(
68 Key key) {
69 return boost::assign::make_list_inserter(
70 boost::assign_detail::call_push_back<This>(*this))(key);
71 }
72
75
77
81 template<class FACTOR_GRAPH>
82 static Ordering Colamd(const FACTOR_GRAPH& graph) {
83 if (graph.empty())
84 return Ordering();
85 else
86 return Colamd(VariableIndex(graph));
87 }
88
90 static GTSAM_EXPORT Ordering Colamd(const VariableIndex& variableIndex);
91
100 template<class FACTOR_GRAPH>
101 static Ordering ColamdConstrainedLast(const FACTOR_GRAPH& graph,
102 const KeyVector& constrainLast, bool forceOrder = false) {
103 if (graph.empty())
104 return Ordering();
105 else
106 return ColamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder);
107 }
108
115 static GTSAM_EXPORT Ordering ColamdConstrainedLast(
116 const VariableIndex& variableIndex, const KeyVector& constrainLast,
117 bool forceOrder = false);
118
127 template<class FACTOR_GRAPH>
128 static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH& graph,
129 const KeyVector& constrainFirst, bool forceOrder = false) {
130 if (graph.empty())
131 return Ordering();
132 else
133 return ColamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder);
134 }
135
143 static GTSAM_EXPORT Ordering ColamdConstrainedFirst(
144 const VariableIndex& variableIndex,
145 const KeyVector& constrainFirst, bool forceOrder = false);
146
156 template<class FACTOR_GRAPH>
157 static Ordering ColamdConstrained(const FACTOR_GRAPH& graph,
158 const FastMap<Key, int>& groups) {
159 if (graph.empty())
160 return Ordering();
161 else
162 return ColamdConstrained(VariableIndex(graph), groups);
163 }
164
172 static GTSAM_EXPORT Ordering ColamdConstrained(
173 const VariableIndex& variableIndex, const FastMap<Key, int>& groups);
174
176 template<class FACTOR_GRAPH>
177 static Ordering Natural(const FACTOR_GRAPH &fg) {
178 KeySet src = fg.keys();
179 KeyVector keys(src.begin(), src.end());
180 std::stable_sort(keys.begin(), keys.end());
181 return Ordering(keys);
182 }
183
185 template<class FACTOR_GRAPH>
186 static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj,
187 std::vector<int>& adj, const FACTOR_GRAPH& graph);
188
190 static GTSAM_EXPORT Ordering Metis(const MetisIndex& met);
191
192 template<class FACTOR_GRAPH>
193 static Ordering Metis(const FACTOR_GRAPH& graph) {
194 if (graph.empty())
195 return Ordering();
196 else
197 return Metis(MetisIndex(graph));
198 }
199
201
203
204 template<class FACTOR_GRAPH>
205 static Ordering Create(OrderingType orderingType,
206 const FACTOR_GRAPH& graph) {
207 if (graph.empty())
208 return Ordering();
209
210 switch (orderingType) {
211 case COLAMD:
212 return Colamd(graph);
213 case METIS:
214 return Metis(graph);
215 case NATURAL:
216 return Natural(graph);
217 case CUSTOM:
218 throw std::runtime_error(
219 "Ordering::Create error: called with CUSTOM ordering type.");
220 default:
221 throw std::runtime_error(
222 "Ordering::Create error: called with unknown ordering type.");
223 }
224 }
225
227
229
230 GTSAM_EXPORT
231 void print(const std::string& str = "", const KeyFormatter& keyFormatter =
232 DefaultKeyFormatter) const;
233
234 GTSAM_EXPORT
235 bool equals(const Ordering& other, double tol = 1e-9) const;
236
238
239private:
241 static GTSAM_EXPORT Ordering ColamdConstrained(
242 const VariableIndex& variableIndex, std::vector<int>& cmember);
243
246 template<class ARCHIVE>
247 void serialize(ARCHIVE & ar, const unsigned int version) {
248 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
249 }
250};
251
253template<> struct traits<Ordering> : public Testable<Ordering> {
254};
255
256}
257
A thin wrapper around std::set that uses boost's fast_pool_allocator.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in METIS a...
Definition: MetisIndex.h:45
Definition: Ordering.h:34
static Ordering Natural(const FACTOR_GRAPH &fg)
Return a natural Ordering. Typically used by iterative solvers.
Definition: Ordering.h:177
static Ordering ColamdConstrained(const FACTOR_GRAPH &graph, const FastMap< Key, int > &groups)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition: Ordering.h:157
Ordering(const KEYS &keys)
Create from a container.
Definition: Ordering.h:55
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition: Ordering.h:82
OrderingType
Type of ordering to use.
Definition: Ordering.h:41
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const KeyVector &constrainLast, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition: Ordering.h:101
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: Ordering.h:46
boost::assign::list_inserter< boost::assign_detail::call_push_back< This > > operator+=(Key key)
Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling push_back.
Definition: Ordering.h:67
Ordering(ITERATOR firstKey, ITERATOR lastKey)
Create an ordering using iterators over keys.
Definition: Ordering.h:61
static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const KeyVector &constrainFirst, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition: Ordering.h:128
GTSAM_EXPORT Ordering()
Create an empty ordering.
Definition: Ordering.h:50
static GTSAM_EXPORT Ordering Metis(const MetisIndex &met)
Compute an ordering determined by METIS from a VariableIndex.
Definition: Ordering.cpp:216
FastMap< Key, size_t > invert() const
Invert (not reverse) the ordering - returns a map from key to order position.
Definition: Ordering.cpp:40
friend class boost::serialization::access
Serialization function.
Definition: Ordering.h:245
Ordering This
Typedef to this class.
Definition: Ordering.h:45
static GTSAM_EXPORT void CSRFormat(std::vector< int > &xadj, std::vector< int > &adj, const FACTOR_GRAPH &graph)
METIS Formatting function.
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43