diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index b865de700..c88948280 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -65,7 +65,6 @@ protected: typedef VerticalBlockView BlockAb; public: - typedef GaussianConditional Conditional; typedef boost::shared_ptr shared_ptr; typedef BlockAb::Block ABlock; @@ -73,6 +72,7 @@ public: typedef BlockAb::Column BVector; typedef BlockAb::constColumn constBVector; +protected: SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix std::vector firstNonzeroBlocks_; AbMatrix matrix_; // the full matrix correponding to the factor @@ -229,9 +229,6 @@ public: void set_firstNonzeroBlocks(size_t row, size_t varpos) { firstNonzeroBlocks_[row] = varpos; } - friend class GaussianFactorGraph; - friend class Inference; - }; // GaussianFactor diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index a0d83be31..106d3bb9f 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -180,8 +180,8 @@ bool GaussianFactorGraph::split(const std::map &M, GaussianFactorG Ab2.push_back(factor); continue; } - Index key1 = factor->keys_[0]; - Index key2 = factor->keys_[1]; + 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)) @@ -210,11 +210,9 @@ bool GaussianFactorGraph::getDiagonalOfHessian(VectorValues &values) const { values.makeZero() ; BOOST_FOREACH( const sharedFactor& factor, factors_) { - Index i = 0 ; - BOOST_FOREACH( const Index& idx, factor->keys_) { - Vector v = columnNormSquare(factor->Ab_(i)) ; - values[idx] += v; - ++i ; + for(GaussianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { + Vector v = columnNormSquare(factor->getA(j)); + values[*j] += v; } } return true ; @@ -233,10 +231,8 @@ void GaussianFactorGraph::multiply(const VectorValues &x, VectorValues &r) const r.makeZero() ; Index i = 0 ; BOOST_FOREACH(const sharedFactor& factor, factors_) { - Index j = 0 ; - BOOST_FOREACH( const Index& idx, factor->keys_ ) { - r[i] += prod(factor->Ab_(j), x[idx]) ; - ++j ; + for(GaussianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { + r[i] += prod(factor->getA(j), x[*j]); } ++i ; } @@ -246,10 +242,8 @@ void GaussianFactorGraph::transposeMultiply(const VectorValues &r, VectorValues x.makeZero() ; Index i = 0 ; BOOST_FOREACH(const sharedFactor& factor, factors_) { - Index j = 0 ; - BOOST_FOREACH( const Index& idx, factor->keys_ ) { - x[idx] += prod(trans(factor->Ab_(j)), r[i]) ; - ++j ; + for(GaussianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { + x[*j] += prod(trans(factor->getA(j)), r[i]) ; } ++i ; }