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

Small details, like using a reference in FOREACH

release/4.3a0
Frank Dellaert 2010-02-21 17:06:11 +00:00
parent fe4471930f
commit 517c82f62f
4 changed files with 25 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cpp/GaussianFactor.cpp
View File

@ -50,7 +50,7 @@ GaussianFactor::GaussianFactor(const vector<shared_ptr> & factors)
// Create RHS and sigmas of right size by adding together row counts
size_t m = 0;
BOOST_FOREACH(shared_ptr factor, factors) m += factor->numberOfRows();
BOOST_FOREACH(const shared_ptr& factor, factors) m += factor->numberOfRows();
b_ = Vector(m);
Vector sigmas(m);
@ -58,7 +58,7 @@ GaussianFactor::GaussianFactor(const vector<shared_ptr> & factors)
// iterate over all factors
bool constrained = false;
BOOST_FOREACH(shared_ptr factor, factors){
BOOST_FOREACH(const shared_ptr& factor, factors){
if (verbose) factor->print();
// number of rows for factor f
const size_t mf = factor->numberOfRows();
@ -452,6 +452,7 @@ GaussianFactor::eliminateMatrix(Matrix& Ab, SharedDiagonal model,
return make_pair(conditional, factor);
}
/* ************************************************************************* */
pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr>
GaussianFactor::eliminate(const Symbol& key) const
{

39
cpp/GaussianFactorGraph.cpp
View File

@ -53,7 +53,7 @@ Errors GaussianFactorGraph::errors(const VectorConfig& x) const {
/* ************************************************************************* */
boost::shared_ptr<Errors> GaussianFactorGraph::errors_(const VectorConfig& x) const {
boost::shared_ptr<Errors> e(new Errors);
BOOST_FOREACH(sharedFactor factor,factors_)
BOOST_FOREACH(const sharedFactor& factor,factors_)
e->push_back(factor->error_vector(x));
return e;
}
@ -61,7 +61,7 @@ boost::shared_ptr<Errors> GaussianFactorGraph::errors_(const VectorConfig& x) co
/* ************************************************************************* */
Errors GaussianFactorGraph::operator*(const VectorConfig& x) const {
Errors e;
BOOST_FOREACH(sharedFactor Ai,factors_)
BOOST_FOREACH(const sharedFactor& Ai,factors_)
e.push_back((*Ai)*x);
return e;
}
@ -75,7 +75,7 @@ void GaussianFactorGraph::multiplyInPlace(const VectorConfig& x, Errors& e) cons
void GaussianFactorGraph::multiplyInPlace(const VectorConfig& x,
const Errors::iterator& e) const {
Errors::iterator ei = e;
BOOST_FOREACH(sharedFactor Ai,factors_) {
BOOST_FOREACH(const sharedFactor& Ai,factors_) {
*ei = (*Ai)*x;
ei++;
}
@ -86,7 +86,7 @@ VectorConfig GaussianFactorGraph::operator^(const Errors& e) const {
VectorConfig x;
// For each factor add the gradient contribution
Errors::const_iterator it = e.begin();
BOOST_FOREACH(sharedFactor Ai,factors_) {
BOOST_FOREACH(const sharedFactor& Ai,factors_) {
VectorConfig xi = (*Ai)^(*(it++));
x.insertAdd(xi);
}
@ -99,7 +99,7 @@ void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
VectorConfig& x) const {
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
BOOST_FOREACH(sharedFactor Ai,factors_)
BOOST_FOREACH(const sharedFactor& Ai,factors_)
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
}
@ -115,7 +115,7 @@ VectorConfig GaussianFactorGraph::gradient(const VectorConfig& x) const {
set<Symbol> GaussianFactorGraph::find_separator(const Symbol& key) const
{
set<Symbol> separator;
BOOST_FOREACH(sharedFactor factor,factors_)
BOOST_FOREACH(const sharedFactor& factor,factors_)
factor->tally_separator(key,separator);
return separator;
@ -137,19 +137,18 @@ GaussianFactorGraph::eliminateOne(const Symbol& key, bool old) {
/* ************************************************************************* */
GaussianConditional::shared_ptr
GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) {
// get a vector of all of the factors in the separator as well as an ordering
// find and remove all factors connected to key
vector<GaussianFactor::shared_ptr> factors = findAndRemoveFactors(key);
// Collect all dimensions as well as the set of separator keys
set<Symbol> separator;
Dimensions dimensions;
BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) {
BOOST_FOREACH(const sharedFactor& factor, factors) {
Dimensions factor_dim = factor->dimensions();
dimensions.insert(factor_dim.begin(), factor_dim.end());
BOOST_FOREACH(const Symbol& k, factor->keys()) {
if (!k.equals(key)) {
BOOST_FOREACH(const Symbol& k, factor->keys())
if (!k.equals(key))
separator.insert(k);
}
}
}
// add the keys to the rendering
@ -160,12 +159,14 @@ GaussianFactorGraph::eliminateOneMatrixJoin(const Symbol& key) {
// combine the factors to get a noisemodel and a combined matrix
Matrix Ab; SharedDiagonal model;
boost::tie(Ab, model) = GaussianFactor::combineFactorsAndCreateMatrix(factors,render,dimensions);
boost::tie(Ab, model) =
GaussianFactor::combineFactorsAndCreateMatrix(factors,render,dimensions);
// eliminate that joint factor
GaussianFactor::shared_ptr factor;
GaussianConditional::shared_ptr conditional;
boost::tie(conditional, factor) = GaussianFactor::eliminateMatrix(Ab, model, render, dimensions);
boost::tie(conditional, factor) =
GaussianFactor::eliminateMatrix(Ab, model, render, dimensions);
// add new factor on separator back into the graph
if (!factor->empty()) push_back(factor);
@ -191,7 +192,7 @@ VectorConfig GaussianFactorGraph::optimize(const Ordering& ordering, bool old)
{
bool verbose = false;
if (verbose)
BOOST_FOREACH(sharedFactor factor,factors_)
BOOST_FOREACH(const sharedFactor& factor,factors_)
factor->get_model()->print("Starting model");
// eliminate all nodes in the given ordering -> chordal Bayes net
@ -245,7 +246,7 @@ GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg
/* ************************************************************************* */
Dimensions GaussianFactorGraph::dimensions() const {
Dimensions result;
BOOST_FOREACH(sharedFactor factor,factors_) {
BOOST_FOREACH(const sharedFactor& factor,factors_) {
Dimensions vs = factor->dimensions();
Symbol key; int dim;
FOREACH_PAIR(key,dim,vs) result.insert(make_pair(key,dim));
@ -277,7 +278,7 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma) const {
/* ************************************************************************* */
Errors GaussianFactorGraph::rhs() const {
Errors e;
BOOST_FOREACH(sharedFactor factor,factors_)
BOOST_FOREACH(const sharedFactor& factor,factors_)
e.push_back(ediv(factor->get_b(),factor->get_sigmas()));
return e;
}
@ -293,7 +294,7 @@ pair<Matrix,Vector> GaussianFactorGraph::matrix(const Ordering& ordering) const
// get all factors
GaussianFactorSet found;
BOOST_FOREACH(sharedFactor factor,factors_)
BOOST_FOREACH(const sharedFactor& factor,factors_)
found.push_back(factor);
// combine them
@ -362,7 +363,7 @@ Matrix GaussianFactorGraph::sparse(const Dimensions& indices) const {
// Collect the I,J,S lists for all factors
int row_index = 0;
BOOST_FOREACH(sharedFactor factor,factors_) {
BOOST_FOREACH(const sharedFactor& factor,factors_) {
// get sparse lists for the factor
list<int> i1,j1;

2
cpp/GaussianFactorGraph.h
View File

@ -129,7 +129,7 @@ namespace gtsam {
* Peforms a supposedly-faster (fewer matrix copy) version of elimination
* CURRENTLY IN TESTING
*/
inline GaussianConditional::shared_ptr eliminateOneMatrixJoin(const Symbol& key);
GaussianConditional::shared_ptr eliminateOneMatrixJoin(const Symbol& key);
/**
* eliminate factor graph in place(!) in the given order, yielding

27
cpp/timeGaussianFactorGraph.cpp
View File

@ -105,33 +105,8 @@ double timePlanarSmootherCombined(int N, size_t reps) {
clock_t start = clock();
for (size_t i = 0; i<reps; ++i) {
// setup
GaussianFactorGraph fg(fgBase);
Ordering render; render += key; // start with variable to eliminate
vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors(key);
set<Symbol> separator;
Dimensions dimensions;
BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) {
Dimensions factor_dim = factor->dimensions();
dimensions.insert(factor_dim.begin(), factor_dim.end());
BOOST_FOREACH(const Symbol& k, factor->keys()) {
if (!k.equals(key)) {
separator.insert(k);
}
}
}
// add the keys to the rendering
BOOST_FOREACH(const Symbol& k, separator)
if (k != key) render += k;
// combine the factors to get a noisemodel and a combined matrix
Matrix Ab; SharedDiagonal model;
boost::tie(Ab, model) = GaussianFactor::combineFactorsAndCreateMatrix(factors,render,dimensions);
fg.eliminateOneMatrixJoin(key);
}
clock_t end = clock ();