91 lines
3.0 KiB
C++
91 lines
3.0 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 VariableIndex.cpp
|
|
* @author Richard Roberts
|
|
* @date Oct 22, 2010
|
|
*/
|
|
|
|
#include <iostream>
|
|
|
|
#include <gtsam/inference/VariableIndex.h>
|
|
#include <gtsam/inference/Permutation.h>
|
|
|
|
namespace gtsam {
|
|
|
|
using namespace std;
|
|
|
|
/* ************************************************************************* */
|
|
bool VariableIndex::equals(const VariableIndex& other, double tol) const {
|
|
if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) {
|
|
for(size_t var=0; var < max(this->index_.size(), other.index_.size()); ++var)
|
|
if(var >= this->index_.size() || var >= other.index_.size()) {
|
|
if(!((var >= this->index_.size() && other.index_[var].empty()) ||
|
|
(var >= other.index_.size() && this->index_[var].empty())))
|
|
return false;
|
|
} else if(this->index_[var] != other.index_[var])
|
|
return false;
|
|
} else
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void VariableIndex::print(const string& str) const {
|
|
cout << str;
|
|
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
|
|
for(Index var = 0; var < size(); ++var) {
|
|
cout << "var " << var << ":";
|
|
BOOST_FOREACH(const size_t factor, index_[var])
|
|
cout << " " << factor;
|
|
cout << "\n";
|
|
}
|
|
cout << flush;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void VariableIndex::outputMetisFormat(ostream& os) const {
|
|
os << size() << " " << nFactors() << "\n";
|
|
// run over variables, which will be hyper-edges.
|
|
BOOST_FOREACH(const Factors& variable, index_) {
|
|
// every variable is a hyper-edge covering its factors
|
|
BOOST_FOREACH(const size_t factor, variable)
|
|
os << (factor+1) << " "; // base 1
|
|
os << "\n";
|
|
}
|
|
os << flush;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void VariableIndex::permuteInPlace(const Permutation& permutation) {
|
|
// Create new index and move references to data into it in permuted order
|
|
deque<VariableIndex::Factors> newIndex(this->size());
|
|
for(Index i = 0; i < newIndex.size(); ++i)
|
|
newIndex[i].swap(this->index_[permutation[i]]);
|
|
|
|
// Move reference to entire index into the VariableIndex
|
|
index_.swap(newIndex);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void VariableIndex::removeUnusedAtEnd(size_t nToRemove) {
|
|
#ifndef NDEBUG
|
|
for(size_t i = this->size() - nToRemove; i < this->size(); ++i)
|
|
if(!(*this)[i].empty())
|
|
throw std::invalid_argument("Attempting to remove non-empty variables with VariableIndex::removeUnusedAtEnd()");
|
|
#endif
|
|
|
|
index_.resize(this->size() - nToRemove);
|
|
}
|
|
|
|
}
|