13 #include "OrderedSymbols.h"
16 namespace gtsam {
namespace partition {
19 template <
class NLG,
class SubNLG,
class GenericGraph>
21 const NLG& fg,
const Ordering&
ordering,
const int numNodeStopPartition,
const int minNodesPerMap,
const bool verbose) :
23 const auto [unaryFactors, gfg] = fg.createGenericGraph(
ordering);
33 keys.reserve(numNodes);
38 root_ =
recursivePartition(gfg, unaryFactors,
keys, vector<size_t>(), numNodeStopPartition, minNodesPerMap, std::shared_ptr<SubNLG>(), workspace, verbose);
42 template <
class NLG,
class SubNLG,
class GenericGraph>
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);
55 keys.reserve(numNodes);
64 template <
class NLG,
class SubNLG,
class GenericGraph>
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]);
71 UnorderedSymbols sepKeys;
72 for(
const size_t index:
sep)
73 sepKeys.insert(int2symbol_[index]);
75 return std::make_shared<SubNLG>(fg, frontalKeys, sepKeys, parent);
79 template <
class NLG,
class SubNLG,
class GenericGraph>
81 const typename GenericGraph::value_type& factor,
const std::vector<int>& partitionTable,
82 vector<GenericGraph>& frontalFactors, NLG& sepFactors, vector<
set<size_t> >& childSeps,
83 typename SubNLG::Weeklinks& weeklinks)
const {
85 int partition1 = partitionTable[factor->key1.index];
86 int partition2 = partitionTable[factor->key2.index];
87 if (partition1 <= 0 && partition2 <= 0) {
88 sepFactors.push_back(fg_[factor->index]);
90 else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) {
91 weeklinks.push_back(fg_[factor->index]);
93 else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) {
94 frontalFactors[partition1 - 1].push_back(factor);
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);
104 throw runtime_error(
"processFactor: unexpected entries in the partition table!");
116 template <
class NLG,
class SubNLG,
class GenericGraph>
119 const std::vector<int>& partitionTable,
const int numSubmaps,
120 vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors,
121 vector<vector<size_t> >& childFrontals, vector<vector<size_t> >& childSeps, vector<size_t>& localFrontals,
122 typename SubNLG::Weeklinks& weeklinks)
const {
126 childFrontals.resize(numSubmaps);
128 partition = partitionTable[
key];
131 case 0: localFrontals.push_back(
key);
break;
132 default: childFrontals[partition-1].push_back(
key);
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);
145 childSeps.push_back(vector<size_t>(childSep.begin(), childSep.end()));
149 partition = partitionTable[unaryFactor_->key.index];
150 if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]);
151 else frontalUnaryFactors[partition-1].push_back(unaryFactor_);
156 template <
class NLG,
class SubNLG,
class GenericGraph>
160 typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end();
161 while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]);
163 sepFactors.push_back(fg_[unaryFactor_->index]);
168 template <
class NLG,
class SubNLG,
class GenericGraph>
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 {
175 if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) {
176 sepFactors = collectOriginalFactors(gfg, unaryFactors);
177 return makeSubNLG(sepFactors, frontals,
sep, parent);
181 int numSubmaps =
findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(),
182 NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
184 if (numSubmaps == 0)
throw runtime_error(
"recursivePartition: get zero submap after ND!");
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);
193 std::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals,
sep, parent);
194 current->setWeeklinks(weeklinks);
197 for (
int i=0;
i<numSubmaps;
i++) {
198 checkSingularity(frontalFactors[
i], childFrontals[
i], workspace, NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
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);
212 template <
class NLG,
class SubNLG,
class GenericGraph>
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 {
220 sepFactors = collectOriginalFactors(gfg, unaryFactors);
221 return makeSubNLG(sepFactors, frontals,
sep, parent);
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);
235 std::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals,
sep, parent);
236 current->setWeeklinks(weeklinks);
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);