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