12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Formatting to default BORG format

release/4.3a0
dellaert 2015-02-13 17:16:38 +01:00
parent 674794d387
commit 56456a2396
3 changed files with 469 additions and 455 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
gtsam/inference/Ordering.cpp
View File

@ -30,8 +30,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
FastMap<Key, size_t> Ordering::invert() const FastMap<Key, size_t> Ordering::invert() const {
{
FastMap<Key, size_t> inverted; FastMap<Key, size_t> inverted;
for (size_t pos = 0; pos < this->size(); ++pos) for (size_t pos = 0; pos < this->size(); ++pos)
inverted.insert(make_pair((*this)[pos], pos)); inverted.insert(make_pair((*this)[pos], pos));
@ -39,23 +38,23 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Ordering Ordering::colamd(const VariableIndex& variableIndex) Ordering Ordering::colamd(const VariableIndex& variableIndex) {
{
// Call constrained version with all groups set to zero // Call constrained version with all groups set to zero
vector<int> dummy_groups(variableIndex.size(), 0); vector<int> dummy_groups(variableIndex.size(), 0);
return Ordering::colamdConstrained(variableIndex, dummy_groups); return Ordering::colamdConstrained(variableIndex, dummy_groups);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Ordering Ordering::colamdConstrained( Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex,
const VariableIndex& variableIndex, std::vector<int>& cmember) std::vector<int>& cmember) {
{
gttic(Ordering_COLAMDConstrained); gttic(Ordering_COLAMDConstrained);
gttic(Prepare); gttic(Prepare);
size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); size_t nEntries = variableIndex.nEntries(), nFactors =
variableIndex.nFactors(), nVars = variableIndex.size();
// Convert to compressed column major format colamd wants it in (== MATLAB format!) // Convert to compressed column major format colamd wants it in (== MATLAB format!)
size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */ size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors,
(int) nVars); /* colamd arg 3: size of the array A */
vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */ vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
@ -95,9 +94,11 @@ namespace gtsam {
/* returns (1) if successful, (0) otherwise*/ /* returns (1) if successful, (0) otherwise*/
if (nVars > 0) { if (nVars > 0) {
gttic(ccolamd); gttic(ccolamd);
int rv = ccolamd((int)nFactors, (int)nVars, (int)Alen, &A[0], &p[0], knobs, stats, &cmember[0]); int rv = ccolamd((int) nFactors, (int) nVars, (int) Alen, &A[0], &p[0],
knobs, stats, &cmember[0]);
if (rv != 1) if (rv != 1)
throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str()); throw runtime_error(
(boost::format("ccolamd failed with return value %1%") % rv).str());
} }
// ccolamd_report(stats); // ccolamd_report(stats);
@ -114,9 +115,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Ordering Ordering::colamdConstrainedLast( Ordering Ordering::colamdConstrainedLast(const VariableIndex& variableIndex,
const VariableIndex& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder) const std::vector<Key>& constrainLast, bool forceOrder) {
{
gttic(Ordering_COLAMDConstrainedLast); gttic(Ordering_COLAMDConstrainedLast);
size_t n = variableIndex.size(); size_t n = variableIndex.size();
@ -141,9 +141,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Ordering Ordering::colamdConstrainedFirst( Ordering Ordering::colamdConstrainedFirst(const VariableIndex& variableIndex,
const VariableIndex& variableIndex, const std::vector<Key>& constrainFirst, bool forceOrder) const std::vector<Key>& constrainFirst, bool forceOrder) {
{
gttic(Ordering_COLAMDConstrainedFirst); gttic(Ordering_COLAMDConstrainedFirst);
const int none = -1; const int none = -1;
@ -176,8 +175,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex, Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex,
const FastMap<Key, int>& groups) const FastMap<Key, int>& groups) {
{
gttic(Ordering_COLAMDConstrained); gttic(Ordering_COLAMDConstrained);
size_t n = variableIndex.size(); size_t n = variableIndex.size();
std::vector<int> cmember(n, 0); std::vector<int> cmember(n, 0);
@ -198,10 +196,8 @@ namespace gtsam {
return Ordering::colamdConstrained(variableIndex, cmember); return Ordering::colamdConstrained(variableIndex, cmember);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Ordering Ordering::metis(const MetisIndex& met) Ordering Ordering::metis(const MetisIndex& met) {
{
gttic(Ordering_METIS); gttic(Ordering_METIS);
vector<idx_t> xadj = met.xadj(); vector<idx_t> xadj = met.xadj();
@ -209,19 +205,18 @@ namespace gtsam {
vector<idx_t> perm, iperm; vector<idx_t> perm, iperm;
idx_t size = met.nValues(); idx_t size = met.nValues();
for (idx_t i = 0; i < size; i++) for (idx_t i = 0; i < size; i++) {
{
perm.push_back(0); perm.push_back(0);
iperm.push_back(0); iperm.push_back(0);
} }
int outputError; int outputError;
outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]); outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0],
&iperm[0]);
Ordering result; Ordering result;
if (outputError != METIS_OK) if (outputError != METIS_OK) {
{
std::cout << "METIS failed during Nested Dissection ordering!\n"; std::cout << "METIS failed during Nested Dissection ordering!\n";
return result; return result;
} }
@ -235,8 +230,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Ordering::print(const std::string& str, const KeyFormatter& keyFormatter) const void Ordering::print(const std::string& str,
{ const KeyFormatter& keyFormatter) const {
cout << str; cout << str;
// Print ordering in index order // Print ordering in index order
// Print the ordering with varsPerLine ordering entries printed on each line, // Print the ordering with varsPerLine ordering entries printed on each line,
@ -262,8 +257,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool Ordering::equals(const Ordering& other, double tol) const bool Ordering::equals(const Ordering& other, double tol) const {
{
return (*this) == other; return (*this) == other;
} }

62
gtsam/inference/Ordering.h
View File

@ -45,21 +45,28 @@ namespace gtsam {
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
/// Create an empty ordering /// Create an empty ordering
GTSAM_EXPORT Ordering() {} GTSAM_EXPORT
Ordering() {
}
/// Create from a container /// Create from a container
template<typename KEYS> template<typename KEYS>
explicit Ordering(const KEYS& keys) : Base(keys.begin(), keys.end()) {} explicit Ordering(const KEYS& keys) :
Base(keys.begin(), keys.end()) {
}
/// Create an ordering using iterators over keys /// Create an ordering using iterators over keys
template<typename ITERATOR> template<typename ITERATOR>
Ordering(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {} Ordering(ITERATOR firstKey, ITERATOR lastKey) :
Base(firstKey, lastKey) {
}
/// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling
/// push_back. /// push_back.
boost::assign::list_inserter<boost::assign_detail::call_push_back<This> > boost::assign::list_inserter<boost::assign_detail::call_push_back<This> > operator+=(
operator+=(Key key) { Key key) {
return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<This>(*this))(key); return boost::assign::make_list_inserter(
boost::assign_detail::call_push_back<This>(*this))(key);
} }
/// Invert (not reverse) the ordering - returns a map from key to order position /// Invert (not reverse) the ordering - returns a map from key to order position
@ -72,7 +79,8 @@ namespace gtsam {
/// it is faster to use COLAMD(const VariableIndex&) /// it is faster to use COLAMD(const VariableIndex&)
template<class FACTOR> template<class FACTOR>
static Ordering colamd(const FactorGraph<FACTOR>& graph) { static Ordering colamd(const FactorGraph<FACTOR>& graph) {
return colamd(VariableIndex(graph)); } return colamd(VariableIndex(graph));
}
/// Compute a fill-reducing ordering using COLAMD from a VariableIndex. /// Compute a fill-reducing ordering using COLAMD from a VariableIndex.
static GTSAM_EXPORT Ordering colamd(const VariableIndex& variableIndex); static GTSAM_EXPORT Ordering colamd(const VariableIndex& variableIndex);
@ -88,7 +96,9 @@ namespace gtsam {
template<class FACTOR> template<class FACTOR>
static Ordering colamdConstrainedLast(const FactorGraph<FACTOR>& graph, static Ordering colamdConstrainedLast(const FactorGraph<FACTOR>& graph,
const std::vector<Key>& constrainLast, bool forceOrder = false) { const std::vector<Key>& constrainLast, bool forceOrder = false) {
return colamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); } return colamdConstrainedLast(VariableIndex(graph), constrainLast,
forceOrder);
}
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This
/// function constrains the variables in \c constrainLast to the end of the ordering, and orders /// function constrains the variables in \c constrainLast to the end of the ordering, and orders
@ -96,8 +106,9 @@ namespace gtsam {
/// variables in \c constrainLast will be ordered in the same order specified in the vector<Key> /// variables in \c constrainLast will be ordered in the same order specified in the vector<Key>
/// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
static GTSAM_EXPORT Ordering colamdConstrainedLast(const VariableIndex& variableIndex, static GTSAM_EXPORT Ordering colamdConstrainedLast(
const std::vector<Key>& constrainLast, bool forceOrder = false); const VariableIndex& variableIndex, const std::vector<Key>& constrainLast,
bool forceOrder = false);
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
/// for note on performance). This internally builds a VariableIndex so if you already have a /// for note on performance). This internally builds a VariableIndex so if you already have a
@ -110,7 +121,9 @@ namespace gtsam {
template<class FACTOR> template<class FACTOR>
static Ordering colamdConstrainedFirst(const FactorGraph<FACTOR>& graph, static Ordering colamdConstrainedFirst(const FactorGraph<FACTOR>& graph,
const std::vector<Key>& constrainFirst, bool forceOrder = false) { const std::vector<Key>& constrainFirst, bool forceOrder = false) {
return colamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); } return colamdConstrainedFirst(VariableIndex(graph), constrainFirst,
forceOrder);
}
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This
/// function constrains the variables in \c constrainFirst to the front of the ordering, and /// function constrains the variables in \c constrainFirst to the front of the ordering, and
@ -119,7 +132,8 @@ namespace gtsam {
/// vector<Key> \c constrainFirst. If \c forceOrder is false, the variables in \c /// vector<Key> \c constrainFirst. If \c forceOrder is false, the variables in \c
/// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to /// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to
/// reduce fill-in as well. /// reduce fill-in as well.
static GTSAM_EXPORT Ordering colamdConstrainedFirst(const VariableIndex& variableIndex, static GTSAM_EXPORT Ordering colamdConstrainedFirst(
const VariableIndex& variableIndex,
const std::vector<Key>& constrainFirst, bool forceOrder = false); const std::vector<Key>& constrainFirst, bool forceOrder = false);
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
@ -134,7 +148,8 @@ namespace gtsam {
template<class FACTOR> template<class FACTOR>
static Ordering colamdConstrained(const FactorGraph<FACTOR>& graph, static Ordering colamdConstrained(const FactorGraph<FACTOR>& graph,
const FastMap<Key, int>& groups) { const FastMap<Key, int>& groups) {
return colamdConstrained(VariableIndex(graph), groups); } return colamdConstrained(VariableIndex(graph), groups);
}
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this
/// function, a group for each variable should be specified in \c groups, and each group of /// function, a group for each variable should be specified in \c groups, and each group of
@ -143,8 +158,8 @@ namespace gtsam {
/// appear in \c groups in arbitrary order. Any variables not present in \c groups will be /// appear in \c groups in arbitrary order. Any variables not present in \c groups will be
/// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the /// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the
/// supplied indices, see the CCOLAMD documentation for more information. /// supplied indices, see the CCOLAMD documentation for more information.
static GTSAM_EXPORT Ordering colamdConstrained(const VariableIndex& variableIndex, static GTSAM_EXPORT Ordering colamdConstrained(
const FastMap<Key, int>& groups); const VariableIndex& variableIndex, const FastMap<Key, int>& groups);
/// Return a natural Ordering. Typically used by iterative solvers /// Return a natural Ordering. Typically used by iterative solvers
template<class FACTOR> template<class FACTOR>
@ -157,14 +172,14 @@ namespace gtsam {
/// METIS Formatting function /// METIS Formatting function
template<class FACTOR> template<class FACTOR>
static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj, std::vector<int>& adj, const FactorGraph<FACTOR>& graph); static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj,
std::vector<int>& adj, const FactorGraph<FACTOR>& graph);
/// Compute an ordering determined by METIS from a VariableIndex /// Compute an ordering determined by METIS from a VariableIndex
static GTSAM_EXPORT Ordering metis(const MetisIndex& met); static GTSAM_EXPORT Ordering metis(const MetisIndex& met);
template<class FACTOR> template<class FACTOR>
static Ordering metis(const FactorGraph<FACTOR>& graph) static Ordering metis(const FactorGraph<FACTOR>& graph) {
{
return metis(MetisIndex(graph)); return metis(MetisIndex(graph));
} }
@ -194,9 +209,12 @@ namespace gtsam {
/// @name Testable @{ /// @name Testable @{
GTSAM_EXPORT void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; GTSAM_EXPORT
void print(const std::string& str = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
GTSAM_EXPORT bool equals(const Ordering& other, double tol = 1e-9) const; GTSAM_EXPORT
bool equals(const Ordering& other, double tol = 1e-9) const;
/// @} /// @}
@ -205,7 +223,6 @@ namespace gtsam {
static GTSAM_EXPORT Ordering colamdConstrained( static GTSAM_EXPORT Ordering colamdConstrained(
const VariableIndex& variableIndex, std::vector<int>& cmember); const VariableIndex& variableIndex, std::vector<int>& cmember);
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
@ -215,7 +232,8 @@ namespace gtsam {
}; };
/// traits /// traits
template<> struct traits<Ordering> : public Testable<Ordering> {}; template<> struct traits<Ordering> : public Testable<Ordering> {
};
} }

16
gtsam/inference/tests/testOrdering.cpp
View File

@ -56,7 +56,8 @@ TEST(Ordering, constrained_ordering) {
EXPECT(assert_equal(expConstrained, actConstrained)); EXPECT(assert_equal(expConstrained, actConstrained));
// constrained version - push one set to the start // constrained version - push one set to the start
Ordering actConstrained2 = Ordering::colamdConstrainedFirst(sfg, list_of(2)(4)); Ordering actConstrained2 = Ordering::colamdConstrainedFirst(sfg,
list_of(2)(4));
Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5)); Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5));
EXPECT(assert_equal(expConstrained2, actConstrained2)); EXPECT(assert_equal(expConstrained2, actConstrained2));
} }
@ -112,9 +113,7 @@ TEST(Ordering, csr_format) {
vector<int> xadjExpected, adjExpected; vector<int> xadjExpected, adjExpected;
xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44; xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44;
adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11, 13, 8, 12, 14, 9, 13;
2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11,
13, 8, 12, 14, 9, 13 ;
EXPECT(xadjExpected == mi.xadj()); EXPECT(xadjExpected == mi.xadj());
EXPECT(adjExpected.size() == mi.adj().size()); EXPECT(adjExpected.size() == mi.adj().size());
@ -243,9 +242,9 @@ TEST(Ordering, MetisLoop) {
{ {
Ordering actual = Ordering::Create(Ordering::METIS, sfg); Ordering actual = Ordering::Create(Ordering::METIS, sfg);
// 0,3 // 0,3
// 1 // 1,3
// 2 // 2
// 4 // 4,0
// 5 // 5
Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3)); Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3));
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -283,5 +282,8 @@ TEST(Ordering, Create) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */