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

Formatting and comments only

release/4.3a0
dellaert 2014-02-13 20:23:06 -05:00
parent b048db4296
commit d70f6b7be4
2 changed files with 32 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/linear/HessianFactor.cpp
View File

@ -62,41 +62,44 @@ string SlotEntry::toString() const {
}
/* ************************************************************************* */
Scatter::Scatter(const GaussianFactorGraph& gfg, boost::optional<const Ordering&> ordering)
{
Scatter::Scatter(const GaussianFactorGraph& gfg,
boost::optional<const Ordering&> ordering) {
gttic(Scatter_Constructor);
static const size_t none = std::numeric_limits<size_t>::max();
// First do the set union.
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) {
if(factor) {
for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) {
if (factor) {
for (GaussianFactor::const_iterator variable = factor->begin();
variable != factor->end(); ++variable) {
// 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)
this->insert(make_pair(*variable, SlotEntry(none, factor->getDim(variable))));
const JacobianFactor* asJacobian =
dynamic_cast<const JacobianFactor*>(factor.get());
if (!asJacobian || asJacobian->cols() > 1)
this->insert(
make_pair(*variable, SlotEntry(none, factor->getDim(variable))));
}
}
}
// If we have an ordering, pre-fill the ordered variables first
size_t slot = 0;
if(ordering) {
if (ordering) {
BOOST_FOREACH(Key key, *ordering) {
const_iterator entry = find(key);
if(entry == end())
if (entry == end())
throw std::invalid_argument(
"The ordering provided to the HessianFactor Scatter constructor\n"
"contained extra variables that did not appear in the factors to combine.");
at(key).slot = (slot ++);
"contained extra variables that did not appear in the factors to combine.");
at(key).slot = (slot++);
}
}
// Next fill in the slot indices (we can only get these after doing the set
// union.
BOOST_FOREACH(value_type& var_slot, *this) {
if(var_slot.second.slot == none)
var_slot.second.slot = (slot ++);
if (var_slot.second.slot == none)
var_slot.second.slot = (slot++);
}
}

25
gtsam/linear/HessianFactor.h
View File

@ -41,21 +41,28 @@ namespace gtsam {
GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys);
// Definition of Scatter, which is an intermediate data structure used when building a
// HessianFactor incrementally, to get the keys in the right order. The "scatter" is a map from
// global variable indices to slot indices in the union of involved variables. We also include
// the dimensionality of the variable.
/**
* One SlotEntry stores the slot index for a variable, as well its dimension.
*/
struct GTSAM_EXPORT SlotEntry {
size_t slot;
size_t dimension;
size_t slot, dimension;
SlotEntry(size_t _slot, size_t _dimension)
: slot(_slot), dimension(_dimension) {}
std::string toString() const;
};
class Scatter : public FastMap<Key, SlotEntry> {
/**
* Scatter is an intermediate data structure used when building a HessianFactor
* incrementally, to get the keys in the right order. The "scatter" is a map from
* global variable indices to slot indices in the union of involved variables.
* We also include the dimensionality of the variable.
*/
class Scatter: public FastMap<Key, SlotEntry> {
public:
Scatter() {}
Scatter(const GaussianFactorGraph& gfg, boost::optional<const Ordering&> ordering = boost::none);
Scatter() {
}
Scatter(const GaussianFactorGraph& gfg,
boost::optional<const Ordering&> ordering = boost::none);
};
/**