From a9370dead425146fb02e10be1467afbab5ded9a1 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 22 Nov 2010 20:54:49 +0000 Subject: [PATCH] Fixed indentation --- gtsam/linear/GaussianFactor.h | 328 +++++++++++++-------------- gtsam/linear/GaussianFactorGraph.cpp | 268 +++++++++++----------- gtsam/linear/GaussianFactorGraph.h | 112 ++++----- 3 files changed, 354 insertions(+), 354 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index a16a7b477..87f46203e 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -47,194 +47,194 @@ namespace gtsam { -class GaussianFactorGraph; + class GaussianFactorGraph; -/** A map from key to dimension, useful in various contexts */ -typedef std::map Dimensions; - -/** - * Base Class for a linear factor. - * GaussianFactor is non-mutable (all methods const!). - * The factor value is exp(-0.5*||Ax-b||^2) - */ -class GaussianFactor: public IndexFactor { - -protected: - - typedef boost::numeric::ublas::matrix AbMatrix; - typedef VerticalBlockView BlockAb; - -public: - typedef GaussianConditional Conditional; - typedef boost::shared_ptr shared_ptr; - typedef BlockAb::Block ABlock; - typedef BlockAb::constBlock constABlock; - typedef BlockAb::Column BVector; - typedef BlockAb::constColumn constBVector; - - enum SolveMethod { SOLVE_QR, SOLVE_CHOLESKY }; - -protected: - SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix - std::vector firstNonzeroBlocks_; - AbMatrix matrix_; // the full matrix correponding to the factor - BlockAb Ab_; // the block view of the full matrix - -public: - - /** Copy constructor */ - GaussianFactor(const GaussianFactor& gf); - - /** default constructor for I/O */ - GaussianFactor(); - - /** Construct Null factor */ - GaussianFactor(const Vector& b_in); - - /** Construct unary factor */ - GaussianFactor(Index i1, const Matrix& A1, - const Vector& b, const SharedDiagonal& model); - - /** Construct binary factor */ - GaussianFactor(Index i1, const Matrix& A1, - Index i2, const Matrix& A2, - const Vector& b, const SharedDiagonal& model); - - /** Construct ternary factor */ - GaussianFactor(Index i1, const Matrix& A1, Index i2, - const Matrix& A2, Index i3, const Matrix& A3, - const Vector& b, const SharedDiagonal& model); - - /** Construct an n-ary factor */ - GaussianFactor(const std::vector > &terms, - const Vector &b, const SharedDiagonal& model); - - GaussianFactor(const std::list > &terms, - const Vector &b, const SharedDiagonal& model); - - /** Construct from Conditional Gaussian */ - GaussianFactor(const GaussianConditional& cg); - - - // Implementing Testable interface - void print(const std::string& s = "") const; - bool equals(const GaussianFactor& lf, double tol = 1e-9) const; - - Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ - Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */ - double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */ - - /** Check if the factor contains no information, i.e. zero rows. This does - * not necessarily mean that the factor involves no variables (to check for - * involving no variables use keys().empty()). - */ - bool empty() const { return Ab_.size1() == 0;} - - /** - * return the number of rows in the corresponding linear system - */ - size_t size1() const { return Ab_.size1(); } - - /** - * return the number of columns in the corresponding linear system - */ - size_t size2() const { return Ab_.size2(); } - - - /** Get a view of the r.h.s. vector b */ - constBVector getb() const { return Ab_.column(size(), 0); } - - /** Get a view of the A matrix for the variable pointed to be the given key iterator */ - constABlock getA(const_iterator variable) const { return Ab_(variable - keys_.begin()); } - - BVector getb() { return Ab_.column(size(), 0); } - - ABlock getA(iterator variable) { return Ab_(variable - keys_.begin()); } - - /** Return the dimension of the variable pointed to by the given key iterator - * todo: Remove this in favor of keeping track of dimensions with variables? - */ - size_t getDim(const_iterator variable) const { return Ab_(variable - keys_.begin()).size2(); } + /** A map from key to dimension, useful in various contexts */ + typedef std::map Dimensions; /** - * Permutes the GaussianFactor, but for efficiency requires the permutation - * to already be inverted. This acts just as a change-of-name for each - * variable. The order of the variables within the factor is not changed. + * Base Class for a linear factor. + * GaussianFactor is non-mutable (all methods const!). + * The factor value is exp(-0.5*||Ax-b||^2) */ - void permuteWithInverse(const Permutation& inversePermutation); + class GaussianFactor: public IndexFactor { - /** - * Named constructor for combining a set of factors with pre-computed set of variables. - */ - static shared_ptr Combine(const FactorGraph& factors, const VariableSlots& variableSlots); + protected: -protected: + typedef boost::numeric::ublas::matrix AbMatrix; + typedef VerticalBlockView BlockAb; - /** Internal debug check to make sure variables are sorted */ - void assertInvariants() const; + public: + typedef GaussianConditional Conditional; + typedef boost::shared_ptr shared_ptr; + typedef BlockAb::Block ABlock; + typedef BlockAb::constBlock constABlock; + typedef BlockAb::Column BVector; + typedef BlockAb::constColumn constBVector; -public: + enum SolveMethod { SOLVE_QR, SOLVE_CHOLESKY }; - /** get a copy of sigmas */ - const Vector& get_sigmas() const { return model_->sigmas(); } + protected: + SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix + std::vector firstNonzeroBlocks_; + AbMatrix matrix_; // the full matrix correponding to the factor + BlockAb Ab_; // the block view of the full matrix - /** get a copy of model */ - const SharedDiagonal& get_model() const { return model_; } + public: - /** get the indices list */ - const std::vector& get_firstNonzeroBlocks() const { return firstNonzeroBlocks_; } + /** Copy constructor */ + GaussianFactor(const GaussianFactor& gf); - /** whether the noise model of this factor is constrained (i.e. contains any sigmas of 0.0) */ - bool isConstrained() const {return model_->isConstrained();} + /** default constructor for I/O */ + GaussianFactor(); - /** - * return the number of rows from the b vector - * @return a integer with the number of rows from the b vector - */ - size_t numberOfRows() const { return Ab_.size1(); } + /** Construct Null factor */ + GaussianFactor(const Vector& b_in); - /** Return A*x */ - Vector operator*(const VectorValues& x) const; + /** Construct unary factor */ + GaussianFactor(Index i1, const Matrix& A1, + const Vector& b, const SharedDiagonal& model); + + /** Construct binary factor */ + GaussianFactor(Index i1, const Matrix& A1, + Index i2, const Matrix& A2, + const Vector& b, const SharedDiagonal& model); + + /** Construct ternary factor */ + GaussianFactor(Index i1, const Matrix& A1, Index i2, + const Matrix& A2, Index i3, const Matrix& A3, + const Vector& b, const SharedDiagonal& model); + + /** Construct an n-ary factor */ + GaussianFactor(const std::vector > &terms, + const Vector &b, const SharedDiagonal& model); + + GaussianFactor(const std::list > &terms, + const Vector &b, const SharedDiagonal& model); + + /** Construct from Conditional Gaussian */ + GaussianFactor(const GaussianConditional& cg); - /** x += A'*e */ - void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const; + // Implementing Testable interface + void print(const std::string& s = "") const; + bool equals(const GaussianFactor& lf, double tol = 1e-9) const; - /** - * Return (dense) matrix associated with factor - * @param ordering of variables needed for matrix column order - * @param set weight to true to bake in the weights - */ - std::pair matrix(bool weight = true) const; + Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ + Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */ + double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */ - /** - * Return (dense) matrix associated with factor - * The returned system is an augmented matrix: [A b] - * @param ordering of variables needed for matrix column order - * @param set weight to use whitening to bake in weights - */ - Matrix matrix_augmented(bool weight = true) const; + /** Check if the factor contains no information, i.e. zero rows. This does + * not necessarily mean that the factor involves no variables (to check for + * involving no variables use keys().empty()). + */ + bool empty() const { return Ab_.size1() == 0;} - /** - * Return vectors i, j, and s to generate an m-by-n sparse matrix - * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. - * As above, the standard deviations are baked into A and b - * @param first column index for each variable - */ - boost::tuple, std::list, std::list > - sparse(const Dimensions& columnIndices) const; + /** + * return the number of rows in the corresponding linear system + */ + size_t size1() const { return Ab_.size1(); } - /* ************************************************************************* */ - // MUTABLE functions. FD:on the path to being eradicated - /* ************************************************************************* */ + /** + * return the number of columns in the corresponding linear system + */ + size_t size2() const { return Ab_.size2(); } - GaussianConditional::shared_ptr eliminateFirst(SolveMethod solveMethod = SOLVE_QR); - GaussianBayesNet::shared_ptr eliminate(size_t nrFrontals = 1, SolveMethod solveMethod = SOLVE_QR); + /** Get a view of the r.h.s. vector b */ + constBVector getb() const { return Ab_.column(size(), 0); } - void set_firstNonzeroBlocks(size_t row, size_t varpos) { firstNonzeroBlocks_[row] = varpos; } + /** Get a view of the A matrix for the variable pointed to be the given key iterator */ + constABlock getA(const_iterator variable) const { return Ab_(variable - keys_.begin()); } -}; // GaussianFactor + BVector getb() { return Ab_.column(size(), 0); } + + ABlock getA(iterator variable) { return Ab_(variable - keys_.begin()); } + + /** Return the dimension of the variable pointed to by the given key iterator + * todo: Remove this in favor of keeping track of dimensions with variables? + */ + size_t getDim(const_iterator variable) const { return Ab_(variable - keys_.begin()).size2(); } + + /** + * Permutes the GaussianFactor, but for efficiency requires the permutation + * to already be inverted. This acts just as a change-of-name for each + * variable. The order of the variables within the factor is not changed. + */ + void permuteWithInverse(const Permutation& inversePermutation); + + /** + * Named constructor for combining a set of factors with pre-computed set of variables. + */ + static shared_ptr Combine(const FactorGraph& factors, const VariableSlots& variableSlots); + + protected: + + /** Internal debug check to make sure variables are sorted */ + void assertInvariants() const; + + public: + + /** get a copy of sigmas */ + const Vector& get_sigmas() const { return model_->sigmas(); } + + /** get a copy of model */ + const SharedDiagonal& get_model() const { return model_; } + + /** get the indices list */ + const std::vector& get_firstNonzeroBlocks() const { return firstNonzeroBlocks_; } + + /** whether the noise model of this factor is constrained (i.e. contains any sigmas of 0.0) */ + bool isConstrained() const {return model_->isConstrained();} + + /** + * return the number of rows from the b vector + * @return a integer with the number of rows from the b vector + */ + size_t numberOfRows() const { return Ab_.size1(); } + + /** Return A*x */ + Vector operator*(const VectorValues& x) const; + + + /** x += A'*e */ + void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const; + + /** + * Return (dense) matrix associated with factor + * @param ordering of variables needed for matrix column order + * @param set weight to true to bake in the weights + */ + std::pair matrix(bool weight = true) const; + + /** + * Return (dense) matrix associated with factor + * The returned system is an augmented matrix: [A b] + * @param ordering of variables needed for matrix column order + * @param set weight to use whitening to bake in weights + */ + Matrix matrix_augmented(bool weight = true) const; + + /** + * Return vectors i, j, and s to generate an m-by-n sparse matrix + * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. + * As above, the standard deviations are baked into A and b + * @param first column index for each variable + */ + boost::tuple, std::list, std::list > + sparse(const Dimensions& columnIndices) const; + + /* ************************************************************************* */ + // MUTABLE functions. FD:on the path to being eradicated + /* ************************************************************************* */ + + GaussianConditional::shared_ptr eliminateFirst(SolveMethod solveMethod = SOLVE_QR); + + GaussianBayesNet::shared_ptr eliminate(size_t nrFrontals = 1, SolveMethod solveMethod = SOLVE_QR); + + void set_firstNonzeroBlocks(size_t row, size_t varpos) { firstNonzeroBlocks_[row] = varpos; } + + }; // GaussianFactor } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index da4d725da..ddcad80ea 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -38,168 +38,168 @@ using namespace gtsam; namespace gtsam { -// Explicitly instantiate so we don't have to include everywhere -INSTANTIATE_FACTOR_GRAPH(GaussianFactor); + // Explicitly instantiate so we don't have to include everywhere + INSTANTIATE_FACTOR_GRAPH(GaussianFactor); -/* ************************************************************************* */ -GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : - FactorGraph (CBN) { -} - -/* ************************************************************************* */ -GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { - std::set, boost::fast_pool_allocator > keys; - BOOST_FOREACH(const sharedFactor& factor, *this) { - if(factor) keys.insert(factor->begin(), factor->end()); } - return keys; -} - -/* ************************************************************************* */ -void GaussianFactorGraph::permuteWithInverse(const Permutation& inversePermutation) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - factor->permuteWithInverse(inversePermutation); + /* ************************************************************************* */ + GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : + FactorGraph (CBN) { } -} -/* ************************************************************************* */ -double GaussianFactorGraph::error(const VectorValues& x) const { - double total_error = 0.; - BOOST_FOREACH(sharedFactor factor,factors_) - total_error += factor->error(x); - return total_error; -} + /* ************************************************************************* */ + GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { + std::set, boost::fast_pool_allocator > keys; + BOOST_FOREACH(const sharedFactor& factor, *this) { + if(factor) keys.insert(factor->begin(), factor->end()); } + return keys; + } -/* ************************************************************************* */ -Errors GaussianFactorGraph::errors(const VectorValues& x) const { - return *errors_(x); -} + /* ************************************************************************* */ + void GaussianFactorGraph::permuteWithInverse(const Permutation& inversePermutation) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + factor->permuteWithInverse(inversePermutation); + } + } -/* ************************************************************************* */ -boost::shared_ptr GaussianFactorGraph::errors_(const VectorValues& x) const { - boost::shared_ptr e(new Errors); - BOOST_FOREACH(const sharedFactor& factor,factors_) - e->push_back(factor->error_vector(x)); - return e; -} + /* ************************************************************************* */ + double GaussianFactorGraph::error(const VectorValues& x) const { + double total_error = 0.; + BOOST_FOREACH(sharedFactor factor,factors_) + total_error += factor->error(x); + return total_error; + } -/* ************************************************************************* */ -Errors GaussianFactorGraph::operator*(const VectorValues& x) const { - Errors e; - BOOST_FOREACH(const sharedFactor& Ai,factors_) - e.push_back((*Ai)*x); - return e; -} + /* ************************************************************************* */ + Errors GaussianFactorGraph::errors(const VectorValues& x) const { + return *errors_(x); + } -/* ************************************************************************* */ -void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const { - multiplyInPlace(x,e.begin()); -} + /* ************************************************************************* */ + boost::shared_ptr GaussianFactorGraph::errors_(const VectorValues& x) const { + boost::shared_ptr e(new Errors); + BOOST_FOREACH(const sharedFactor& factor,factors_) + e->push_back(factor->error_vector(x)); + return e; + } -/* ************************************************************************* */ -void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, - const Errors::iterator& e) const { - Errors::iterator ei = e; - BOOST_FOREACH(const sharedFactor& Ai,factors_) { - *ei = (*Ai)*x; - ei++; - } -} + /* ************************************************************************* */ + Errors GaussianFactorGraph::operator*(const VectorValues& x) const { + Errors e; + BOOST_FOREACH(const sharedFactor& Ai,factors_) + e.push_back((*Ai)*x); + return e; + } + + /* ************************************************************************* */ + void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const { + multiplyInPlace(x,e.begin()); + } + + /* ************************************************************************* */ + void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, + const Errors::iterator& e) const { + Errors::iterator ei = e; + BOOST_FOREACH(const sharedFactor& Ai,factors_) { + *ei = (*Ai)*x; + ei++; + } + } -/* ************************************************************************* */ -// x += alpha*A'*e -void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, - VectorValues& x) const { - // For each factor add the gradient contribution - Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const sharedFactor& Ai,factors_) - Ai->transposeMultiplyAdd(alpha,*(ei++),x); -} + /* ************************************************************************* */ + // x += alpha*A'*e + void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, + VectorValues& x) const { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const sharedFactor& Ai,factors_) + Ai->transposeMultiplyAdd(alpha,*(ei++),x); + } -/* ************************************************************************* */ -VectorValues GaussianFactorGraph::gradient(const VectorValues& x) const { - // It is crucial for performance to make a zero-valued clone of x - VectorValues g = VectorValues::zero(x); - transposeMultiplyAdd(1.0, errors(x), g); - return g; -} + /* ************************************************************************* */ + VectorValues GaussianFactorGraph::gradient(const VectorValues& x) const { + // It is crucial for performance to make a zero-valued clone of x + VectorValues g = VectorValues::zero(x); + transposeMultiplyAdd(1.0, errors(x), g); + return g; + } -/* ************************************************************************* */ -void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg){ - for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){ - push_back(*factor); - } -} + /* ************************************************************************* */ + void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg){ + for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){ + push_back(*factor); + } + } -/* ************************************************************************* */ -GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg1, - const GaussianFactorGraph& lfg2) { + /* ************************************************************************* */ + GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg1, + const GaussianFactorGraph& lfg2) { - // create new linear factor graph equal to the first one - GaussianFactorGraph fg = lfg1; + // create new linear factor graph equal to the first one + GaussianFactorGraph fg = lfg1; - // add the second factors_ in the graph - for (const_iterator factor = lfg2.factors_.begin(); factor - != lfg2.factors_.end(); factor++) { - fg.push_back(*factor); - } - return fg; -} + // add the second factors_ in the graph + for (const_iterator factor = lfg2.factors_.begin(); factor + != lfg2.factors_.end(); factor++) { + fg.push_back(*factor); + } + return fg; + } -void GaussianFactorGraph::residual(const VectorValues &x, VectorValues &r) const { + void GaussianFactorGraph::residual(const VectorValues &x, VectorValues &r) const { - getb(r) ; - VectorValues Ax = VectorValues::SameStructure(r) ; - multiply(x,Ax) ; - axpy(-1.0,Ax,r) ; -} + getb(r) ; + VectorValues Ax = VectorValues::SameStructure(r) ; + multiply(x,Ax) ; + axpy(-1.0,Ax,r) ; + } -void GaussianFactorGraph::multiply(const VectorValues &x, VectorValues &r) const { + void GaussianFactorGraph::multiply(const VectorValues &x, VectorValues &r) const { - r.makeZero() ; - Index i = 0 ; - BOOST_FOREACH(const sharedFactor& factor, factors_) { - for(GaussianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { - r[i] += prod(factor->getA(j), x[*j]); - } - ++i ; - } -} + r.makeZero() ; + Index i = 0 ; + BOOST_FOREACH(const sharedFactor& factor, factors_) { + for(GaussianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { + r[i] += prod(factor->getA(j), x[*j]); + } + ++i ; + } + } -void GaussianFactorGraph::transposeMultiply(const VectorValues &r, VectorValues &x) const { - x.makeZero() ; - Index i = 0 ; - BOOST_FOREACH(const sharedFactor& factor, factors_) { - for(GaussianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { - x[*j] += prod(trans(factor->getA(j)), r[i]) ; - } - ++i ; - } -} + void GaussianFactorGraph::transposeMultiply(const VectorValues &r, VectorValues &x) const { + x.makeZero() ; + Index i = 0 ; + BOOST_FOREACH(const sharedFactor& factor, factors_) { + for(GaussianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { + x[*j] += prod(trans(factor->getA(j)), r[i]) ; + } + ++i ; + } + } -VectorValues GaussianFactorGraph::allocateVectorValuesb() const { + VectorValues GaussianFactorGraph::allocateVectorValuesb() const { std::vector dimensions(size()) ; Index i = 0 ; BOOST_FOREACH( const sharedFactor& factor, factors_) { - dimensions[i] = factor->numberOfRows() ; - i++; + dimensions[i] = factor->numberOfRows() ; + i++; } return VectorValues(dimensions) ; -} + } -void GaussianFactorGraph::getb(VectorValues &b) const { - Index i = 0 ; - BOOST_FOREACH( const sharedFactor& factor, factors_) { - b[i] = factor->getb() ; - i++; - } -} + void GaussianFactorGraph::getb(VectorValues &b) const { + Index i = 0 ; + BOOST_FOREACH( const sharedFactor& factor, factors_) { + b[i] = factor->getb() ; + i++; + } + } -VectorValues GaussianFactorGraph::getb() const { - VectorValues b = allocateVectorValuesb() ; - getb(b) ; - return b ; -} + VectorValues GaussianFactorGraph::getb() const { + VectorValues b = allocateVectorValuesb() ; + getb(b) ; + return b ; + } } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index b4b36a7ba..f8738f638 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -16,7 +16,7 @@ * @author Christian Potthast * @author Alireza Fathi */ - + #pragma once #include @@ -55,43 +55,43 @@ namespace gtsam { push_back(fg); } - /* dummy constructor, to be compatible with conjugate gradient solver */ + /* dummy constructor, to be compatible with conjugate gradient solver */ template GaussianFactorGraph(const FactorGraph& fg, const VectorValues &x0) { push_back(fg); } - /** Add a null factor */ + /** Add a null factor */ void add(const Vector& b) { - push_back(sharedFactor(new GaussianFactor(b))); - } + push_back(sharedFactor(new GaussianFactor(b))); + } - /** Add a unary factor */ + /** Add a unary factor */ void add(Index key1, const Matrix& A1, - const Vector& b, const SharedDiagonal& model) { - push_back(sharedFactor(new GaussianFactor(key1,A1,b,model))); - } + const Vector& b, const SharedDiagonal& model) { + push_back(sharedFactor(new GaussianFactor(key1,A1,b,model))); + } - /** Add a binary factor */ + /** Add a binary factor */ void add(Index key1, const Matrix& A1, - Index key2, const Matrix& A2, - const Vector& b, const SharedDiagonal& model) { - push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,b,model))); - } + Index key2, const Matrix& A2, + const Vector& b, const SharedDiagonal& model) { + push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,b,model))); + } - /** Add a ternary factor */ + /** Add a ternary factor */ void add(Index key1, const Matrix& A1, - Index key2, const Matrix& A2, - Index key3, const Matrix& A3, - const Vector& b, const SharedDiagonal& model) { - push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,key3,A3,b,model))); - } + Index key2, const Matrix& A2, + Index key3, const Matrix& A3, + const Vector& b, const SharedDiagonal& model) { + push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,key3,A3,b,model))); + } - /** Add an n-ary factor */ + /** Add an n-ary factor */ void add(const std::vector > &terms, - const Vector &b, const SharedDiagonal& model) { - push_back(sharedFactor(new GaussianFactor(terms,b,model))); - } + const Vector &b, const SharedDiagonal& model) { + push_back(sharedFactor(new GaussianFactor(terms,b,model))); + } /** * Return the set of variables involved in the factors (computes a set @@ -103,38 +103,38 @@ namespace gtsam { /** Permute the variables in the factors */ void permuteWithInverse(const Permutation& inversePermutation); - /** return A*x-b */ - Errors errors(const VectorValues& x) const; + /** return A*x-b */ + Errors errors(const VectorValues& x) const; - /** shared pointer version */ - boost::shared_ptr errors_(const VectorValues& x) const; + /** shared pointer version */ + boost::shared_ptr errors_(const VectorValues& x) const; - /** unnormalized error */ - double error(const VectorValues& x) const; + /** unnormalized error */ + double error(const VectorValues& x) const; - /** return A*x */ - Errors operator*(const VectorValues& x) const; + /** return A*x */ + Errors operator*(const VectorValues& x) const; - /* In-place version e <- A*x that overwrites e. */ - void multiplyInPlace(const VectorValues& x, Errors& e) const; + /* In-place version e <- A*x that overwrites e. */ + void multiplyInPlace(const VectorValues& x, Errors& e) const; - /* In-place version e <- A*x that takes an iterator. */ - void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const; + /* In-place version e <- A*x that takes an iterator. */ + void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const; - /** x += alpha*A'*e */ - void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const; + /** x += alpha*A'*e */ + void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const; - /** - * Calculate Gradient of A^(A*x-b) for a given config - * @param x: VectorValues specifying where to calculate gradient - * @return gradient, as a VectorValues as well - */ - VectorValues gradient(const VectorValues& x) const; + /** + * Calculate Gradient of A^(A*x-b) for a given config + * @param x: VectorValues specifying where to calculate gradient + * @return gradient, as a VectorValues as well + */ + VectorValues gradient(const VectorValues& x) const; - /** Unnormalized probability. O(n) */ - double probPrime(const VectorValues& c) const { - return exp(-0.5 * error(c)); - } + /** Unnormalized probability. O(n) */ + double probPrime(const VectorValues& c) const { + return exp(-0.5 * error(c)); + } /** * static function that combines two factor graphs @@ -143,8 +143,8 @@ namespace gtsam { * @return a new combined factor graph */ static GaussianFactorGraph combine2(const GaussianFactorGraph& lfg1, - const GaussianFactorGraph& lfg2); - + const GaussianFactorGraph& lfg2); + /** * combine two factor graphs * @param *lfg Linear factor graph @@ -152,13 +152,13 @@ namespace gtsam { void combine(const GaussianFactorGraph &lfg); // 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 ; + 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 ; + // get b + void getb(VectorValues &b) const ; + VectorValues getb() const ; // allocate a vectorvalues of b's structure VectorValues allocateVectorValuesb() const ;