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

27
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); gttic(Scatter_Constructor);
static const size_t none = std::numeric_limits<size_t>::max(); static const size_t none = std::numeric_limits<size_t>::max();
// First do the set union. // First do the set union.
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) {
if(factor) { if (factor) {
for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { 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 // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers
const JacobianFactor* asJacobian = dynamic_cast<const JacobianFactor*>(factor.get()); const JacobianFactor* asJacobian =
if(!asJacobian || asJacobian->cols() > 1) dynamic_cast<const JacobianFactor*>(factor.get());
this->insert(make_pair(*variable, SlotEntry(none, factor->getDim(variable)))); 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 // If we have an ordering, pre-fill the ordered variables first
size_t slot = 0; size_t slot = 0;
if(ordering) { if (ordering) {
BOOST_FOREACH(Key key, *ordering) { BOOST_FOREACH(Key key, *ordering) {
const_iterator entry = find(key); const_iterator entry = find(key);
if(entry == end()) if (entry == end())
throw std::invalid_argument( throw std::invalid_argument(
"The ordering provided to the HessianFactor Scatter constructor\n" "The ordering provided to the HessianFactor Scatter constructor\n"
"contained extra variables that did not appear in the factors to combine."); "contained extra variables that did not appear in the factors to combine.");
at(key).slot = (slot ++); at(key).slot = (slot++);
} }
} }
// Next fill in the slot indices (we can only get these after doing the set // Next fill in the slot indices (we can only get these after doing the set
// union. // union.
BOOST_FOREACH(value_type& var_slot, *this) { BOOST_FOREACH(value_type& var_slot, *this) {
if(var_slot.second.slot == none) if (var_slot.second.slot == none)
var_slot.second.slot = (slot ++); 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> > GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys); 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 * One SlotEntry stores the slot index for a variable, as well its dimension.
// global variable indices to slot indices in the union of involved variables. We also include */
// the dimensionality of the variable.
struct GTSAM_EXPORT SlotEntry { struct GTSAM_EXPORT SlotEntry {
size_t slot; size_t slot, dimension;
size_t dimension;
SlotEntry(size_t _slot, size_t _dimension) SlotEntry(size_t _slot, size_t _dimension)
: slot(_slot), dimension(_dimension) {} : slot(_slot), dimension(_dimension) {}
std::string toString() const; 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: public:
Scatter() {} Scatter() {
Scatter(const GaussianFactorGraph& gfg, boost::optional<const Ordering&> ordering = boost::none); }
Scatter(const GaussianFactorGraph& gfg,
boost::optional<const Ordering&> ordering = boost::none);
}; };
/** /**