gtsam 4.2.0
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
53 using KeyVector::KeyVector; // Inherit the KeyVector's constructors
54
56 template<typename KEYS>
57 explicit Ordering(const KEYS& keys) :
58 Base(keys.begin(), keys.end()) {
59 }
60
63 boost::assign::list_inserter<boost::assign_detail::call_push_back<This> > operator+=(
64 Key key) {
65 return boost::assign::make_list_inserter(
66 boost::assign_detail::call_push_back<This>(*this))(key);
67 }
68
76
78 bool contains(const Key& key) const;
79
87
90
94 template<class FACTOR_GRAPH>
95 static Ordering Colamd(const FACTOR_GRAPH& graph) {
96 if (graph.empty())
97 return Ordering();
98 else
99 return Colamd(VariableIndex(graph));
100 }
101
103 static GTSAM_EXPORT Ordering Colamd(const VariableIndex& variableIndex);
104
113 template<class FACTOR_GRAPH>
114 static Ordering ColamdConstrainedLast(const FACTOR_GRAPH& graph,
115 const KeyVector& constrainLast, bool forceOrder = false) {
116 if (graph.empty())
117 return Ordering();
118 else
119 return ColamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder);
120 }
121
128 static GTSAM_EXPORT Ordering ColamdConstrainedLast(
129 const VariableIndex& variableIndex, const KeyVector& constrainLast,
130 bool forceOrder = false);
131
140 template<class FACTOR_GRAPH>
141 static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH& graph,
142 const KeyVector& constrainFirst, bool forceOrder = false) {
143 if (graph.empty())
144 return Ordering();
145 else
146 return ColamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder);
147 }
148
156 static GTSAM_EXPORT Ordering ColamdConstrainedFirst(
157 const VariableIndex& variableIndex,
158 const KeyVector& constrainFirst, bool forceOrder = false);
159
169 template<class FACTOR_GRAPH>
170 static Ordering ColamdConstrained(const FACTOR_GRAPH& graph,
171 const FastMap<Key, int>& groups) {
172 if (graph.empty())
173 return Ordering();
174 else
175 return ColamdConstrained(VariableIndex(graph), groups);
176 }
177
185 static GTSAM_EXPORT Ordering ColamdConstrained(
186 const VariableIndex& variableIndex, const FastMap<Key, int>& groups);
187
189 template<class FACTOR_GRAPH>
190 static Ordering Natural(const FACTOR_GRAPH &fg) {
191 KeySet src = fg.keys();
192 KeyVector keys(src.begin(), src.end());
193 std::stable_sort(keys.begin(), keys.end());
194 return Ordering(keys.begin(), keys.end());
195 }
196
198 template<class FACTOR_GRAPH>
199 static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj,
200 std::vector<int>& adj, const FACTOR_GRAPH& graph);
201
203 static GTSAM_EXPORT Ordering Metis(const MetisIndex& met);
204
205 template<class FACTOR_GRAPH>
206 static Ordering Metis(const FACTOR_GRAPH& graph) {
207 if (graph.empty())
208 return Ordering();
209 else
210 return Metis(MetisIndex(graph));
211 }
212
214
217
218 template<class FACTOR_GRAPH>
219 static Ordering Create(OrderingType orderingType,
220 const FACTOR_GRAPH& graph) {
221 if (graph.empty())
222 return Ordering();
223
224 switch (orderingType) {
225 case COLAMD:
226 return Colamd(graph);
227 case METIS:
228 return Metis(graph);
229 case NATURAL:
230 return Natural(graph);
231 case CUSTOM:
232 throw std::runtime_error(
233 "Ordering::Create error: called with CUSTOM ordering type.");
234 default:
235 throw std::runtime_error(
236 "Ordering::Create error: called with unknown ordering type.");
237 }
238 }
239
241
244
245 GTSAM_EXPORT
246 void print(const std::string& str = "", const KeyFormatter& keyFormatter =
247 DefaultKeyFormatter) const;
248
249 GTSAM_EXPORT
250 bool equals(const Ordering& other, double tol = 1e-9) const;
251
253
254private:
256 static GTSAM_EXPORT Ordering ColamdConstrained(
257 const VariableIndex& variableIndex, std::vector<int>& cmember);
258
261 template<class ARCHIVE>
262 void serialize(ARCHIVE & ar, const unsigned int version) {
263 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
264 }
265};
266
268template<> struct traits<Ordering> : public Testable<Ordering> {
269};
270
271}
272
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:100
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:190
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:170
Ordering(const KEYS &keys)
Create from a container.
Definition: Ordering.h:57
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:95
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:114
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:63
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:141
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:212
FastMap< Key, size_t > invert() const
Invert (not reverse) the ordering - returns a map from key to order position.
Definition: Ordering.cpp:36
friend class boost::serialization::access
Serialization function.
Definition: Ordering.h:260
Ordering This
Typedef to this class.
Definition: Ordering.h:45
bool contains(const Key &key) const
Check if key exists in ordering.
Definition: Ordering.cpp:293
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