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

Scatter class in separate compilation unit

release/4.3a0
Frank Dellaert 2015-06-11 21:21:01 -07:00
parent 29c2b47ace
commit 257060e8dd
7 changed files with 200 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
gtsam/linear/HessianFactor.cpp
View File

@ -53,55 +53,6 @@ namespace br { using namespace boost::range; using namespace boost::adaptors; }
namespace gtsam {
/* ************************************************************************* */
string SlotEntry::toString() const {
ostringstream oss;
oss << "SlotEntry: slot=" << slot << ", dim=" << dimension;
return oss.str();
}
/* ************************************************************************* */
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) {
// 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)
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) {
BOOST_FOREACH(Key key, *ordering) {
const_iterator entry = find(key);
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++);
}
}
// 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++);
}
}
/* ************************************************************************* */
HessianFactor::HessianFactor() :
info_(cref_list_of<1>(1))

28
gtsam/linear/HessianFactor.h
View File

@ -18,10 +18,10 @@
#pragma once
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/Scatter.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/linear/GaussianFactor.h>
#include <boost/make_shared.hpp>
@ -41,30 +41,6 @@ namespace gtsam {
GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys);
/**
* One SlotEntry stores the slot index for a variable, as well its dimension.
*/
struct GTSAM_EXPORT SlotEntry {
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);
};
/**
* @brief A Gaussian factor using the canonical parameters (information form)
*

2
gtsam/linear/JacobianFactor.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/Scatter.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/VariableSlots.h>

77
gtsam/linear/Scatter.cpp Normal file
View File

@ -0,0 +1,77 @@
/* ----------------------------------------------------------------------------
* 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 HessianFactor.cpp
* @author Richard Roberts
* @date Dec 8, 2010
*/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/Scatter.h>
#include <gtsam/inference/Ordering.h>
#include <boost/foreach.hpp>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
string SlotEntry::toString() const {
ostringstream oss;
oss << "SlotEntry: slot=" << slot << ", dim=" << dimension;
return oss.str();
}
/* ************************************************************************* */
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) {
// 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)
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) {
BOOST_FOREACH(Key key, *ordering) {
const_iterator entry = find(key);
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++);
}
}
// 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++);
}
}
} // gtsam

56
gtsam/linear/Scatter.h Normal file
View File

@ -0,0 +1,56 @@
/* ----------------------------------------------------------------------------
* 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.h
* @brief Maps global variable indices to slot indices
* @author Richard Roberts
* @author Frank Dellaert
* @date Dec 8, 2010
*/
#pragma once
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/dllexport.h>
#include <boost/optional.hpp>
namespace gtsam {
class GaussianFactorGraph;
class Ordering;
/**
* One SlotEntry stores the slot index for a variable, as well its dimension.
*/
struct GTSAM_EXPORT SlotEntry {
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(const GaussianFactorGraph& gfg,
boost::optional<const Ordering&> ordering = boost::none);
};
} // \ namespace gtsam

2
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file testCholeskyFactor.cpp
* @file testHessianFactor.cpp
* @author Richard Roberts
* @date Dec 15, 2010
*/

63
gtsam/linear/tests/testScatter.cpp Normal file
View File

@ -0,0 +1,63 @@
/* ----------------------------------------------------------------------------
* 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 testScatter.cpp
* @author Frank Dellaert
* @date June, 2015
*/
#include <gtsam/linear/Scatter.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate) {
static const size_t m = 3, n = 3;
Matrix A01 =
(Matrix(m, n) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished();
Vector3 b0(1.5, 1.5, 1.5);
Vector3 s0(1.6, 1.6, 1.6);
Matrix A10 =
(Matrix(m, n) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0).finished();
Matrix A11 = (Matrix(m, n) << -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0)
.finished();
Vector3 b1(2.5, 2.5, 2.5);
Vector3 s1(2.6, 2.6, 2.6);
Matrix A21 =
(Matrix(m, n) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0).finished();
Vector3 b2(3.5, 3.5, 3.5);
Vector3 s2(3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Scatter scatter(gfg);
EXPECT_LONGS_EQUAL(2, scatter.size());
EXPECT_LONGS_EQUAL(0, scatter.at(0).slot);
EXPECT_LONGS_EQUAL(1, scatter.at(1).slot);
EXPECT_LONGS_EQUAL(n, scatter.at(0).dimension);
EXPECT_LONGS_EQUAL(n, scatter.at(1).dimension);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */