gtsam
Loading...
Searching...
No Matches
NestedDissection-inl.h
1/*
2 * NestedDissection-inl.h
3 *
4 * Created on: Nov 27, 2010
5 * Author: nikai
6 * Description:
7 */
8
9#pragma once
10
11
12#include "partition/FindSeparator-inl.h"
13#include "OrderedSymbols.h"
14#include "NestedDissection.h"
15
16namespace gtsam { namespace partition {
17
18 /* ************************************************************************* */
19 template <class NLG, class SubNLG, class GenericGraph>
20 NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
21 const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) :
22 fg_(fg), ordering_(ordering){
23 const auto [unaryFactors, gfg] = fg.createGenericGraph(ordering);
24
25 // build reverse mapping from integer to symbol
26 int numNodes = ordering.size();
27 int2symbol_.resize(numNodes);
28 Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
29 while(it != itLast)
30 int2symbol_[it->second] = (it++)->first;
31
32 vector<size_t> keys;
33 keys.reserve(numNodes);
34 for(int i=0; i<ordering.size(); ++i)
35 keys.push_back(i);
36
37 WorkSpace workspace(numNodes);
38 root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), numNodeStopPartition, minNodesPerMap, std::shared_ptr<SubNLG>(), workspace, verbose);
39 }
40
41 /* ************************************************************************* */
42 template <class NLG, class SubNLG, class GenericGraph>
43 NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
44 const NLG& fg, const Ordering& ordering, const std::shared_ptr<Cuts>& cuts, const bool verbose) : fg_(fg), ordering_(ordering){
45 const auto [unaryFactors, gfg] = fg.createGenericGraph(ordering);
46
47 // build reverse mapping from integer to symbol
48 int numNodes = ordering.size();
49 int2symbol_.resize(numNodes);
50 Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
51 while(it != itLast)
52 int2symbol_[it->second] = (it++)->first;
53
54 vector<size_t> keys;
55 keys.reserve(numNodes);
56 for(int i=0; i<ordering.size(); ++i)
57 keys.push_back(i);
58
59 WorkSpace workspace(numNodes);
60 root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), cuts, std::shared_ptr<SubNLG>(), workspace, verbose);
61 }
62
63 /* ************************************************************************* */
64 template <class NLG, class SubNLG, class GenericGraph>
65 std::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::makeSubNLG(
66 const NLG& fg, const vector<size_t>& frontals, const vector<size_t>& sep, const std::shared_ptr<SubNLG>& parent) const {
67 OrderedSymbols frontalKeys;
68 for(const size_t index: frontals)
69 frontalKeys.push_back(int2symbol_[index]);
70
71 UnorderedSymbols sepKeys;
72 for(const size_t index: sep)
73 sepKeys.insert(int2symbol_[index]);
74
75 return std::make_shared<SubNLG>(fg, frontalKeys, sepKeys, parent);
76 }
77
78 /* ************************************************************************* */
79 template <class NLG, class SubNLG, class GenericGraph>
80 void NestedDissection<NLG, SubNLG, GenericGraph>::processFactor(
81 const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable, // input
82 vector<GenericGraph>& frontalFactors, NLG& sepFactors, vector<set<size_t> >& childSeps, // output factor graphs
83 typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques
84 list<size_t> sep_; // the separator variables involved in the current factor
85 int partition1 = partitionTable[factor->key1.index];
86 int partition2 = partitionTable[factor->key2.index];
87 if (partition1 <= 0 && partition2 <= 0) { // is a factor in the current clique
88 sepFactors.push_back(fg_[factor->index]);
89 }
90 else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) { // is a weeklink (factor between two child cliques)
91 weeklinks.push_back(fg_[factor->index]);
92 }
93 else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques
94 frontalFactors[partition1 - 1].push_back(factor);
95 }
96 else { // is a joint factor in the child clique (involving varaibles in the current clique)
97 if (partition1 > 0 && partition2 <= 0) {
98 frontalFactors[partition1 - 1].push_back(factor);
99 childSeps[partition1 - 1].insert(factor->key2.index);
100 } else if (partition1 <= 0 && partition2 > 0) {
101 frontalFactors[partition2 - 1].push_back(factor);
102 childSeps[partition2 - 1].insert(factor->key1.index);
103 } else
104 throw runtime_error("processFactor: unexpected entries in the partition table!");
105 }
106 }
107
108 /* ************************************************************************* */
115 // TODO: frontalFactors and localFrontals should be generated in findSeparator
116 template <class NLG, class SubNLG, class GenericGraph>
117 void NestedDissection<NLG, SubNLG, GenericGraph>::partitionFactorsAndVariables(
118 const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& keys, //input
119 const std::vector<int>& partitionTable, const int numSubmaps, // input
120 vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors, // output factor graphs
121 vector<vector<size_t> >& childFrontals, vector<vector<size_t> >& childSeps, vector<size_t>& localFrontals, // output sub-orderings
122 typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques
123
124 // make three lists of variables A, B, and C
125 int partition;
126 childFrontals.resize(numSubmaps);
127 for(const size_t key: keys){
128 partition = partitionTable[key];
129 switch (partition) {
130 case -1: break; // the separator of the separator variables
131 case 0: localFrontals.push_back(key); break; // the separator variables
132 default: childFrontals[partition-1].push_back(key); // the frontal variables
133 }
134 }
135
136 // group the factors to {frontalFactors} and {sepFactors},and find the joint variables
137 vector<set<size_t> > childSeps_;
138 childSeps_.resize(numSubmaps);
139 childSeps.reserve(numSubmaps);
140 frontalFactors.resize(numSubmaps);
141 frontalUnaryFactors.resize(numSubmaps);
142 for(typename GenericGraph::value_type factor: fg)
143 processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks);
144 for(const set<size_t>& childSep: childSeps_)
145 childSeps.push_back(vector<size_t>(childSep.begin(), childSep.end()));
146
147 // add unary factor to the current cluster or pass it to one of the child clusters
148 for(const sharedGenericUnaryFactor& unaryFactor_: unaryFactors) {
149 partition = partitionTable[unaryFactor_->key.index];
150 if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]);
151 else frontalUnaryFactors[partition-1].push_back(unaryFactor_);
152 }
153 }
154
155 /* ************************************************************************* */
156 template <class NLG, class SubNLG, class GenericGraph>
157 NLG NestedDissection<NLG, SubNLG, GenericGraph>::collectOriginalFactors(
158 const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const {
159 NLG sepFactors;
160 typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end();
161 while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]);
162 for(const sharedGenericUnaryFactor& unaryFactor_: unaryFactors)
163 sepFactors.push_back(fg_[unaryFactor_->index]);
164 return sepFactors;
165 }
166
167 /* ************************************************************************* */
168 template <class NLG, class SubNLG, class GenericGraph>
169 std::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
170 const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
171 const int numNodeStopPartition, const int minNodesPerMap, const std::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
172
173 // if no split needed
174 NLG sepFactors; // factors that should remain in the current cluster
175 if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) {
176 sepFactors = collectOriginalFactors(gfg, unaryFactors);
177 return makeSubNLG(sepFactors, frontals, sep, parent);
178 }
179
180 // find the nested dissection separator
181 int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(),
182 NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
183 partition::PartitionTable& partitionTable = workspace.partitionTable;
184 if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!");
185
186 // split the factors between child cliques and the current clique
187 vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
188 vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
189 partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
190 frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
191
192 // make a new cluster
193 std::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
194 current->setWeeklinks(weeklinks);
195
196 // check whether all the submaps are fully constrained
197 for (int i=0; i<numSubmaps; i++) {
198 checkSingularity(frontalFactors[i], childFrontals[i], workspace, NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
199 }
200
201 // create child clusters
202 for (int i=0; i<numSubmaps; i++) {
203 std::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
204 numNodeStopPartition, minNodesPerMap, current, workspace, verbose);
205 current->addChild(child);
206 }
207
208 return current;
209 }
210
211 /* ************************************************************************* */
212 template <class NLG, class SubNLG, class GenericGraph>
213 std::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
214 const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
215 const std::shared_ptr<Cuts>& cuts, const std::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
216
217 // if there is no need to cut any more
218 NLG sepFactors; // factors that should remain in the current cluster
219 if (!cuts.get()) {
220 sepFactors = collectOriginalFactors(gfg, unaryFactors);
221 return makeSubNLG(sepFactors, frontals, sep, parent);
222 }
223
224 // retrieve the current partitioning info
225 int numSubmaps = 2;
226 partition::PartitionTable& partitionTable = cuts->partitionTable;
227
228 // split the factors between child cliques and the current clique
229 vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
230 vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
231 partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
232 frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
233
234 // make a new cluster
235 std::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
236 current->setWeeklinks(weeklinks);
237
238 // create child clusters
239 for (int i=0; i<2; i++) {
240 std::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
241 cuts->children.empty() ? std::shared_ptr<Cuts>() : cuts->children[i], current, workspace, verbose);
242 current->addChild(child);
243 }
244 return current;
245 }
246}} //namespace
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
vector
STL class.