66 lines
1.5 KiB
C
66 lines
1.5 KiB
C
|
/* ----------------------------------------------------------------------------
|
||
|
|
|
||
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||
|
|
* Atlanta, Georgia 30332-0415
|
||
|
|
* All Rights Reserved
|
||
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||
|
|
|
||
|
|
* See LICENSE for the license information
|
||
|
|
|
||
|
|
* -------------------------------------------------------------------------- */
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @file DSFMap.h
|
||
|
|
* @date Oct 26, 2013
|
||
|
|
* @author Frank Dellaert
|
||
|
|
* @brief Allow for arbitrary type in DSF
|
||
|
|
*/
|
||
|
|
|
||
|
|
#pragma once
|
||
|
|
|
||
|
|
#include <map>
|
||
|
|
|
||
|
|
namespace gtsam {
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Disjoint set forest using an STL map data structure underneath
|
||
|
|
* Uses rank compression but not union by rank :-(
|
||
|
|
* @addtogroup base
|
||
|
|
*/
|
||
|
|
template<class KEY>
|
||
|
|
class DSFMap {
|
||
|
|
|
||
|
|
/// We store the forest in an STL map
|
||
|
|
typedef std::map<KEY, KEY> Map;
|
||
|
|
mutable Map parent_;
|
||
|
|
|
||
|
|
public:
|
||
|
|
/// constructor
|
||
|
|
DSFMap() {}
|
||
|
|
|
||
|
|
/// find the label of the set in which {key} lives
|
||
|
|
KEY find(const KEY& key) const {
|
||
|
|
typename Map::const_iterator it = parent_.find(key);
|
||
|
|
// if key does not exist, create and return itself
|
||
|
|
if (it==parent_.end()) {
|
||
|
|
parent_[key] = key;
|
||
|
|
return key;
|
||
|
|
} else {
|
||
|
|
// follow parent pointers until we reach set representative
|
||
|
|
KEY parent = it->second;
|
||
|
|
if (parent != key)
|
||
|
|
parent = find(parent); // not yet, recurse!
|
||
|
|
parent_[key] = parent; // path compression
|
||
|
|
return parent;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/// Merge two sets
|
||
|
|
void merge(const KEY& i1, const KEY& i2) {
|
||
|
|
parent_[find(i2)] = find(i1);
|
||
|
|
}
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
}
|