94 lines
2.7 KiB
C++
94 lines
2.7 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 Scatter.cpp
|
|
* @author Richard Roberts
|
|
* @author Frank Dellaert
|
|
* @date June 2015
|
|
*/
|
|
|
|
#include <gtsam/linear/GaussianFactorGraph.h>
|
|
#include <gtsam/linear/Scatter.h>
|
|
#include <gtsam/inference/Ordering.h>
|
|
|
|
#include <algorithm>
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
string SlotEntry::toString() const {
|
|
ostringstream oss;
|
|
oss << "SlotEntry: key=" << key << ", dim=" << dimension;
|
|
return oss.str();
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Scatter::Scatter(const GaussianFactorGraph& gfg,
|
|
boost::optional<const Ordering&> ordering) {
|
|
gttic(Scatter_Constructor);
|
|
|
|
// If we have an ordering, pre-fill the ordered variables first
|
|
if (ordering) {
|
|
for (Key key : *ordering) {
|
|
add(key, 0);
|
|
}
|
|
}
|
|
|
|
// Now, find dimensions of variables and/or extend
|
|
for (const auto& factor : gfg) {
|
|
if (!factor)
|
|
continue;
|
|
|
|
// TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers
|
|
const JacobianFactor* asJacobian = dynamic_cast<const JacobianFactor*>(factor.get());
|
|
if (asJacobian && asJacobian->cols() <= 1) continue;
|
|
|
|
// loop over variables
|
|
for (GaussianFactor::const_iterator variable = factor->begin();
|
|
variable != factor->end(); ++variable) {
|
|
const Key key = *variable;
|
|
iterator it = find(key); // theoretically expensive, yet cache friendly
|
|
if (it!=end())
|
|
it->dimension = factor->getDim(variable);
|
|
else
|
|
add(key, factor->getDim(variable));
|
|
}
|
|
}
|
|
|
|
// To keep the same behavior as before, sort the keys after the ordering
|
|
iterator first = begin();
|
|
if (ordering) first += ordering->size();
|
|
if (first != end()) std::sort(first, end());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void Scatter::add(Key key, size_t dim) {
|
|
emplace_back(SlotEntry(key, dim));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
FastVector<SlotEntry>::iterator Scatter::find(Key key) {
|
|
iterator it = begin();
|
|
while(it != end()) {
|
|
if (it->key == key)
|
|
return it;
|
|
++it;
|
|
}
|
|
return it; // end()
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
} // gtsam
|