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

13
gtsam/linear/HessianFactor.cpp
View File

@ -62,19 +62,22 @@ 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) {
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());
const JacobianFactor* asJacobian =
dynamic_cast<const JacobianFactor*>(factor.get());
if (!asJacobian || asJacobian->cols() > 1)
this->insert(make_pair(*variable, SlotEntry(none, factor->getDim(variable))));
this->insert(
make_pair(*variable, SlotEntry(none, factor->getDim(variable))));
}
}
}

23
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;
};
/**
* 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);
};
/**