77 lines
2.6 KiB
C++
77 lines
2.6 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
|
|
* @brief
|
|
* @author Richard Roberts
|
|
* @created Oct 22, 2010
|
|
*/
|
|
|
|
#include <gtsam/inference/VariableIndex.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
void VariableIndex::permute(const Permutation& permutation) {
|
|
#ifndef NDEBUG
|
|
// Assert that the permutation does not leave behind any non-empty variables,
|
|
// otherwise the nFactors and nEntries counts would be incorrect.
|
|
for(Index j=0; j<this->index_.size(); ++j)
|
|
if(find(permutation.begin(), permutation.end(), j) == permutation.end())
|
|
assert(this->operator[](j).empty());
|
|
#endif
|
|
index_.permute(permutation);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void VariableIndex::rebaseFactors(ptrdiff_t baseIndexChange) {
|
|
BOOST_FOREACH(Factors& factors, index_.container()) {
|
|
BOOST_FOREACH(size_t& factor, factors) {
|
|
factor += baseIndexChange;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
bool VariableIndex::equals(const VariableIndex& other, double tol) const {
|
|
if(this->nEntries_ == other.nEntries_ && this->nFactors_ == other.nFactors_) {
|
|
for(size_t var=0; var < std::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 std::string& str) const {
|
|
std::cout << str;
|
|
Index var = 0;
|
|
BOOST_FOREACH(const Factors& variable, index_.container()) {
|
|
Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var);
|
|
assert(rvar != index_.permutation().end());
|
|
std::cout << "var " << (rvar-index_.permutation().begin()) << ":";
|
|
BOOST_FOREACH(const size_t factor, variable) {
|
|
std::cout << " " << factor;
|
|
}
|
|
std::cout << "\n";
|
|
++ var;
|
|
}
|
|
std::cout << std::flush;
|
|
}
|
|
|
|
}
|