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

Qualified boost::shared_ptr

release/4.3a0
Richard Roberts 2012-06-11 14:53:13 +00:00
parent 22c8c483a9
commit e53aecf970
1 changed files with 9 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/linear/GaussianBayesNet.cpp
View File

@ -28,7 +28,6 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using boost::shared_ptr;
// trick from some reading group // trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
@ -73,13 +72,13 @@ void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R,
} }
/* ************************************************************************* */ /* ************************************************************************* */
shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn) { boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn) {
vector<size_t> dimensions(bn.size()); vector<size_t> dimensions(bn.size());
Index var = 0; Index var = 0;
BOOST_FOREACH(const shared_ptr<const GaussianConditional> conditional, bn) { BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> conditional, bn) {
dimensions[var++] = conditional->dim(); dimensions[var++] = conditional->dim();
} }
return shared_ptr<VectorValues>(new VectorValues(dimensions)); return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -93,7 +92,7 @@ VectorValues optimize(const GaussianBayesNet& bn) {
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) { void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) {
/** solve each node in turn in topological sort order (parents first)*/ /** solve each node in turn in topological sort order (parents first)*/
BOOST_REVERSE_FOREACH(const shared_ptr<const GaussianConditional> cg, bn) { BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
// i^th part of R*x=y, x=inv(R)*y // i^th part of R*x=y, x=inv(R)*y
// (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:)) // (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
cg->solveInPlace(x); cg->solveInPlace(x);
@ -103,14 +102,14 @@ void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) {
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& input) { VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& input) {
VectorValues output = input; VectorValues output = input;
BOOST_REVERSE_FOREACH(const shared_ptr<const GaussianConditional> cg, bn) { BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
const Index key = *(cg->beginFrontals()); const Index key = *(cg->beginFrontals());
Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents()); Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents());
xS = input[key] - cg->get_S() * xS; xS = input[key] - cg->get_S() * xS;
output[key] = cg->get_R().triangularView<Eigen::Upper>().solve(xS); output[key] = cg->get_R().triangularView<Eigen::Upper>().solve(xS);
} }
BOOST_FOREACH(const shared_ptr<const GaussianConditional> cg, bn) { BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
cg->scaleFrontalsBySigma(output); cg->scaleFrontalsBySigma(output);
} }
@ -131,7 +130,7 @@ VectorValues backSubstituteTranspose(const GaussianBayesNet& bn,
// we loop from first-eliminated to last-eliminated // we loop from first-eliminated to last-eliminated
// i^th part of L*gy=gx is done block-column by block-column of L // i^th part of L*gy=gx is done block-column by block-column of L
BOOST_FOREACH(const shared_ptr<const GaussianConditional> cg, bn) BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn)
cg->solveTransposeInPlace(gy); cg->solveTransposeInPlace(gy);
// Scale gy // Scale gy
@ -197,7 +196,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
Index key; size_t I; Index key; size_t I;
FOREACH_PAIR(key,I,mapping) { FOREACH_PAIR(key,I,mapping) {
// find corresponding conditional // find corresponding conditional
shared_ptr<const GaussianConditional> cg = bn[key]; boost::shared_ptr<const GaussianConditional> cg = bn[key];
// get sigmas // get sigmas
Vector sigmas = cg->get_sigmas(); Vector sigmas = cg->get_sigmas();
@ -234,7 +233,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
double determinant(const GaussianBayesNet& bayesNet) { double determinant(const GaussianBayesNet& bayesNet) {
double logDet = 0.0; double logDet = 0.0;
BOOST_FOREACH(shared_ptr<const GaussianConditional> cg, bayesNet){ BOOST_FOREACH(boost::shared_ptr<const GaussianConditional> cg, bayesNet){
logDet += cg->get_R().diagonal().unaryExpr(ptr_fun<double,double>(log)).sum(); logDet += cg->get_R().diagonal().unaryExpr(ptr_fun<double,double>(log)).sum();
} }