18 namespace gtsam {
namespace partition {
24 const int minNrConstraintsPerCamera,
const int minNrConstraintsPerLandmark)
26 typedef pair<int, int> IntPair;
27 typedef list<sharedGenericFactor2D> FactorList;
28 typedef map<IntPair, FactorList::iterator> Connections;
34 size_t nrFactors =
factors.size();
35 FactorList::iterator itEnd;
38 Connections connections;
41 list<FactorList::iterator> toErase;
42 for (FactorList::iterator itFactor=
factors.begin(); itFactor!=itEnd; itFactor++) {
47 toErase.push_back(itFactor); nrFactors--;
continue;
50 size_t label1 = dsf.
find(
key1.index);
51 size_t label2 = dsf.
find(
key2.index);
52 if (label1 == label2) { toErase.push_back(itFactor); nrFactors--;
continue; }
57 toErase.push_back(itFactor); nrFactors--;
58 dsf.
merge(label1, label2);
66 toErase.push_back(itFactor); nrFactors--;
67 dsf.
merge(label1, label2);
73 IntPair
labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1);
74 Connections::iterator itCached = connections.find(
labels);
75 if (itCached == connections.end()) {
76 connections.insert(make_pair(
labels, itFactor));
79 GenericNode2D key21 = (*itCached->second)->
key1, key22 = (*itCached->second)->key2;
85 toErase.push_back(itFactor); nrFactors--;
88 toErase.push_back(itFactor); nrFactors--;
89 toErase.push_back(itCached->second); nrFactors--;
90 dsf.
merge(label1, label2);
91 connections.erase(itCached);
99 for(
const FactorList::iterator& it: toErase)
105 list<vector<size_t> > islands;
106 map<size_t, vector<size_t> > arrays = dsf.
arrays();
107 for(
const auto& kv : arrays)
108 islands.push_back(kv.second);
115 cout <<
name << endl;
117 cout << factor_->key1.index <<
" " << factor_->key2.index << endl;
122 cout <<
name << endl;
124 cout << factor_->key1.index <<
" " << factor_->key2.index <<
" (" <<
125 factor_->key1.type <<
", " << factor_->key2.type <<
")" << endl;
132 typedef list<sharedGenericFactor3D> FactorList;
135 size_t nrFactors =
factors.size();
136 FactorList::iterator itEnd;
139 bool succeed =
false;
141 list<FactorList::iterator> toErase;
142 for (FactorList::iterator itFactor=
factors.begin(); itFactor!=itEnd; itFactor++) {
145 if (
graph.size() == 178765) cout <<
"kai21" << endl;
147 if (
graph.size() == 178765) cout <<
"kai21: " <<
key1.index <<
" " <<
key2.index << endl;
149 toErase.push_back(itFactor); nrFactors--;
continue;
152 if (
graph.size() == 178765) cout <<
"kai22" << endl;
153 size_t label1 = dsf.
find(
key1.index);
154 size_t label2 = dsf.
find(
key2.index);
155 if (label1 == label2) { toErase.push_back(itFactor); nrFactors--;
continue; }
157 if (
graph.size() == 178765) cout <<
"kai23" << endl;
162 toErase.push_back(itFactor); nrFactors--;
163 dsf.
merge(label1, label2);
168 if (
graph.size() == 178765) cout <<
"kai24" << endl;
174 for(
const FactorList::iterator& it: toErase)
187 return singularCameras.find(node.
index) != singularCameras.end();
break;
189 return singularLandmarks.find(node.
index) != singularLandmarks.end();
break;
191 throw runtime_error(
"unrecognized key type!");
197 const vector<bool>& isCamera,
const vector<bool>& isLandmark,
198 set<size_t>& singularCameras, set<size_t>& singularLandmarks, vector<int>& nrConstraints,
199 bool& foundSingularCamera,
bool& foundSingularLandmark,
200 const int minNrConstraintsPerCamera,
const int minNrConstraintsPerLandmark) {
203 std::fill(nrConstraints.begin(), nrConstraints.end(), 0);
205 const int&
key1 = factor_->key1.index;
206 const int&
key2 = factor_->key2.index;
208 !
isSingular(singularCameras, singularLandmarks, factor_->key1) &&
209 !
isSingular(singularCameras, singularLandmarks, factor_->key2)) {
210 nrConstraints[
key1]++;
211 nrConstraints[
key2]++;
216 nrConstraints[
key1]+=2;
217 nrConstraints[
key2]+=2;
223 foundSingularCamera =
false;
224 foundSingularLandmark =
false;
225 for (
size_t i=0;
i<nrConstraints.size();
i++) {
226 if (isCamera[
i] && nrConstraints[
i] < minNrConstraintsPerCamera &&
227 singularCameras.find(
i) == singularCameras.end()) {
228 singularCameras.insert(
i);
229 foundSingularCamera =
true;
231 if (isLandmark[
i] && nrConstraints[
i] < minNrConstraintsPerLandmark &&
232 singularLandmarks.find(
i) == singularLandmarks.end()) {
233 singularLandmarks.insert(
i);
234 foundSingularLandmark =
true;
241 const size_t minNrConstraintsPerCamera,
const size_t minNrConstraintsPerLandmark) {
247 const bool verbose =
false;
248 bool foundSingularCamera =
true;
249 bool foundSingularLandmark =
true;
251 list<vector<size_t> > islands;
252 set<size_t> singularCameras, singularLandmarks;
253 vector<bool> isCamera(workspace.
dictionary.size(),
false);
254 vector<bool> isLandmark(workspace.
dictionary.size(),
false);
260 if (workspace.
dictionary[factor_->key1.index] != -1) {
262 isCamera[factor_->key1.index] =
true;
264 isLandmark[factor_->key1.index] =
true;
266 if (workspace.
dictionary[factor_->key2.index] != -1) {
268 isCamera[factor_->key2.index] =
true;
270 isLandmark[factor_->key2.index] =
true;
274 vector<int> nrConstraints(workspace.
dictionary.size(), 0);
277 while (foundSingularCamera || foundSingularLandmark) {
279 singularCameras, singularLandmarks, nrConstraints,
280 foundSingularCamera, foundSingularLandmark,
281 minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
285 if (!singularCameras.empty()) {
286 if (verbose) cout <<
"singular cameras:";
287 for(
const size_t i: singularCameras) {
288 islands.push_back(vector<size_t>(1,
i));
289 if (verbose) cout <<
i <<
" ";
291 if (verbose) cout << endl;
293 if (!singularLandmarks.empty()) {
294 if (verbose) cout <<
"singular landmarks:";
295 for(
const size_t i: singularLandmarks) {
296 islands.push_back(vector<size_t>(1,
i));
297 if (verbose) cout <<
i <<
" ";
299 if (verbose) cout << endl;
304 map<size_t, vector<size_t> > labelIslands = dsf.
arrays();
305 size_t label; vector<size_t> island;
306 for(
const auto& li: labelIslands) {
307 tie(label, island) = li;
308 vector<size_t> filteredIsland;
309 filteredIsland.reserve(island.size());
310 for(
const size_t key: island) {
311 if ((isCamera[
key] && singularCameras.find(
key) == singularCameras.end()) ||
312 (isLandmark[
key] && singularLandmarks.find(
key) == singularLandmarks.end()) ||
313 (!isCamera[
key] && !isLandmark[
key])) {
314 filteredIsland.push_back(
key);
317 islands.push_back(filteredIsland);
322 for(
const vector<size_t>& island: islands)
325 cout <<
nrKeys <<
" vs " <<
keys.size() << endl;
326 throw runtime_error(
"findIslands: the number of keys is inconsistent!");
330 if (verbose) cout <<
"found " << islands.size() <<
" islands!" << endl;
337 size_t i1 = 0, i2 = 0;
338 int nrCommonLandmarks = 0;
339 while (
i1 < landmarks1.size() && i2 < landmarks2.size()) {
340 if (landmarks1[
i1] < landmarks2[i2])
342 else if (landmarks1[
i1] > landmarks2[i2])
346 nrCommonLandmarks ++;
349 return nrCommonLandmarks;
354 const std::vector<int>& dictionary,
GenericGraph3D& reducedGraph) {
356 typedef size_t LandmarkKey;
358 vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());
360 vector<int> cameraToCamera(dictionary.size(), -1);
365 cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index);
368 if (factor_->key1.index < factor_->key2.index) {
369 key_i = factor_->key1.index;
370 key_j = factor_->key2.index;
372 key_i = factor_->key2.index;
373 key_j = factor_->key1.index;
375 cameraToCamera[key_i] = key_j;
381 for(vector<LandmarkKey> &landmarks: cameraToLandmarks){
382 if (!landmarks.empty())
383 std::sort(landmarks.begin(), landmarks.end());
387 reducedGraph.clear();
391 for (
size_t i1=0;
i1<cameraKeys.size()-1; ++
i1) {
392 for (
size_t i2=
i1+1; i2<cameraKeys.size(); ++i2) {
397 if (nrCommonLandmarks > 0 || hasOdometry) {
398 nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0);
399 reducedGraph.push_back(std::make_shared<GenericFactor3D>(
camera1,
camera2,
408 WorkSpace& workspace,
const size_t minNrConstraintsPerCamera,
const size_t minNrConstraintsPerLandmark) {
410 vector<size_t> nrConstraints(workspace.
dictionary.size(), 0);
413 const vector<int>& dictionary = workspace.
dictionary;
414 vector<bool> isValidCamera(workspace.
dictionary.size(),
false);
415 vector<bool> isValidLandmark(workspace.
dictionary.size(),
false);
419 const size_t&
key1 = factor_->key1.index;
420 const size_t&
key2 = factor_->key2.index;
421 if (dictionary[
key1] == -1 || dictionary[
key2] == -1)
424 isValidCamera[
key1] =
true;
426 isValidLandmark[
key2] =
true;
428 isValidCamera[
key2] =
true;
430 nrConstraints[
key1]++;
431 nrConstraints[
key2]++;
436 nrConstraints[
key1]+=2;
437 nrConstraints[
key2]+=2;
442 size_t minFoundConstraintsPerCamera = 10000;
443 size_t minFoundConstraintsPerLandmark = 10000;
445 for (
size_t i=0;
i<isValidCamera.size();
i++) {
446 if (isValidCamera[
i]) {
447 minFoundConstraintsPerCamera =
std::min(nrConstraints[
i], minFoundConstraintsPerCamera);
448 if (nrConstraints[
i] < minNrConstraintsPerCamera)
449 cout <<
"!!!!!!!!!!!!!!!!!!! camera with " << nrConstraints[
i] <<
" constraint: " <<
i << endl;
453 for (
size_t j=0;
j<isValidLandmark.size();
j++) {
454 if (isValidLandmark[
j]) {
455 minFoundConstraintsPerLandmark =
std::min(nrConstraints[
j], minFoundConstraintsPerLandmark);
456 if (nrConstraints[
j] < minNrConstraintsPerLandmark)
457 cout <<
"!!!!!!!!!!!!!!!!!!! landmark with " << nrConstraints[
j] <<
" constraint: " <<
j << endl;
462 for(
const size_t key: frontals) {
463 if (isValidCamera[
key] && nrConstraints[
key] < minNrConstraintsPerCamera)
464 cout <<
"singular camera:" <<
key <<
" with " << nrConstraints[
key] <<
" constraints" << endl;
467 if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera)
468 throw runtime_error(
"checkSingularity:minConstraintsPerCamera < " + std::to_string(minFoundConstraintsPerCamera));
469 if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark)
470 throw runtime_error(
"checkSingularity:minConstraintsPerLandmark < " + std::to_string(minFoundConstraintsPerLandmark));