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

take out two spcg specific functions from gaussian factor graph

release/4.3a0
Yong-Dian Jian 2010-11-16 17:17:40 +00:00
parent d6929d4409
commit 1193d2f9be
3 changed files with 56 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -146,60 +146,6 @@ GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg
return fg;
}
bool GaussianFactorGraph::split(const std::map<Index, Index> &M, GaussianFactorGraph &Ab1, GaussianFactorGraph &Ab2) const {
//typedef sharedFactor F ;
Ab1 = GaussianFactorGraph();
Ab2 = GaussianFactorGraph();
BOOST_FOREACH(const sharedFactor& factor, factors_) {
if (factor->keys().size() > 2)
throw(invalid_argument("split: only support factors with at most two keys"));
if (factor->keys().size() == 1) {
Ab1.push_back(factor);
Ab2.push_back(factor);
continue;
}
Index key1 = factor->keys()[0];
Index key2 = factor->keys()[1];
if ((M.find(key1) != M.end() && M.find(key1)->second == key2) ||
(M.find(key2) != M.end() && M.find(key2)->second == key1))
Ab1.push_back(factor);
else
Ab2.push_back(factor);
}
return true ;
}
VectorValues GaussianFactorGraph::allocateVectorValuesb() const {
std::vector<size_t> dimensions(size()) ;
Index i = 0 ;
BOOST_FOREACH( const sharedFactor& factor, factors_) {
dimensions[i] = factor->numberOfRows() ;
i++;
}
return VectorValues(dimensions) ;
}
bool GaussianFactorGraph::getDiagonalOfHessian(VectorValues &values) const {
values.makeZero() ;
BOOST_FOREACH( const sharedFactor& factor, factors_) {
for(GaussianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
Vector v = columnNormSquare(factor->getA(j));
values[*j] += v;
}
}
return true ;
}
void GaussianFactorGraph::residual(const VectorValues &x, VectorValues &r) const {
getb(r) ;
@ -231,6 +177,17 @@ void GaussianFactorGraph::transposeMultiply(const VectorValues &r, VectorValues
}
}
VectorValues GaussianFactorGraph::allocateVectorValuesb() const {
std::vector<size_t> dimensions(size()) ;
Index i = 0 ;
BOOST_FOREACH( const sharedFactor& factor, factors_) {
dimensions[i] = factor->numberOfRows() ;
i++;
}
return VectorValues(dimensions) ;
}
void GaussianFactorGraph::getb(VectorValues &b) const {
Index i = 0 ;
BOOST_FOREACH( const sharedFactor& factor, factors_) {

19
gtsam/linear/GaussianFactorGraph.h
View File

@ -151,23 +151,18 @@ namespace gtsam {
*/
void combine(const GaussianFactorGraph &lfg);
/**
* Split a Gaussian factor graph into two, according to M
* M keeps the vertex indices of edges of A1. The others belong to A2.
*/
bool split(const std::map<Index, Index> &M, GaussianFactorGraph &A1, GaussianFactorGraph &A2) const ;
// allocate a vectorvalues of b's structure
VectorValues allocateVectorValuesb() const ;
/* get the diagonal of A^ A, used to build jacobi preconditioner */
bool getDiagonalOfHessian(VectorValues &values) const ;
// matrix-vector operations
void residual(const VectorValues &x, VectorValues &r) const ;
void multiply(const VectorValues &x, VectorValues &r) const ;
void transposeMultiply(const VectorValues &r, VectorValues &x) const ;
// get b
void getb(VectorValues &b) const ;
VectorValues getb() const ;
// allocate a vectorvalues of b's structure
VectorValues allocateVectorValuesb() const ;
};
} // namespace gtsam

39
gtsam/linear/SubgraphSolver-inl.h
View File

@ -11,11 +11,14 @@
#pragma once
#include <map>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/linear/iterative-inl.h>
@ -26,6 +29,40 @@ using namespace std;
namespace gtsam {
/* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */
bool split(const std::map<Index, Index> &M,
const GaussianFactorGraph &Ab,
GaussianFactorGraph &Ab1,
GaussianFactorGraph &Ab2) {
Ab1 = GaussianFactorGraph();
Ab2 = GaussianFactorGraph();
for ( size_t i = 0 ; i < Ab.size() ; ++i ) {
boost::shared_ptr<GaussianFactor> factor = Ab[i] ;
if (factor->keys().size() > 2)
throw(invalid_argument("split: only support factors with at most two keys"));
if (factor->keys().size() == 1) {
Ab1.push_back(factor);
Ab2.push_back(factor);
continue;
}
Index key1 = factor->keys()[0];
Index key2 = factor->keys()[1];
if ((M.find(key1) != M.end() && M.find(key1)->second == key2) ||
(M.find(key2) != M.end() && M.find(key2)->second == key1))
Ab1.push_back(factor);
else
Ab2.push_back(factor);
}
return true ;
}
template<class GRAPH, class LINEAR, class VALUES>
void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
@ -33,7 +70,7 @@ void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::
Ab2 = boost::make_shared<LINEAR>();
if (parameters_->verbosity()) cout << "split the graph ...";
graph->split(pairs_, *Ab1, *Ab2) ;
split(pairs_, *graph, *Ab1, *Ab2) ;
if (parameters_->verbosity()) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl;
// // Add a HardConstraint to the root, otherwise the root will be singular