move linear algebra functions for jacobian factor graph to a new file
							parent
							
								
									c518381373
								
							
						
					
					
						commit
						a9c36fc172
					
				|  | @ -19,7 +19,7 @@ | |||
| #include <boost/foreach.hpp> | ||||
| #include <boost/tuple/tuple.hpp> | ||||
| 
 | ||||
| #include <gtsam/linear/JacobianFactor.h> | ||||
| #include <gtsam/linear/JacobianFactorGraph.h> | ||||
| #include <gtsam/linear/GaussianBayesNet.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| 
 | ||||
|  |  | |||
|  | @ -18,7 +18,7 @@ | |||
|  */ | ||||
| 
 | ||||
| #include <gtsam/linear/GaussianBayesTree.h> | ||||
| #include <gtsam/linear/JacobianFactor.h> | ||||
| #include <gtsam/linear/JacobianFactorGraph.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|  |  | |||
|  | @ -536,98 +536,4 @@ namespace gtsam { | |||
| 			model_ = noiseModel::Diagonal::Sigmas(sigmas); | ||||
| 	} | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x) { | ||||
|     Errors e; | ||||
|     BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { | ||||
|       e.push_back((*Ai)*x); | ||||
|     } | ||||
|     return e; | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, Errors& e) { | ||||
|     multiplyInPlace(fg,x,e.begin()); | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const Errors::iterator& e) { | ||||
|     Errors::iterator ei = e; | ||||
|     BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { | ||||
|       *ei = (*Ai)*x; | ||||
|       ei++; | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   // x += alpha*A'*e
 | ||||
|   void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, const Errors& e, VectorValues& x) { | ||||
|     // For each factor add the gradient contribution
 | ||||
|     Errors::const_iterator ei = e.begin(); | ||||
|     BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { | ||||
|       Ai->transposeMultiplyAdd(alpha,*(ei++),x); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x0) { | ||||
|     // It is crucial for performance to make a zero-valued clone of x
 | ||||
|     VectorValues g = VectorValues::Zero(x0); | ||||
|     Errors e; | ||||
|     BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { | ||||
|       e.push_back(factor->error_vector(x0)); | ||||
|     } | ||||
|     transposeMultiplyAdd(fg, 1.0, e, g); | ||||
|     return g; | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g) { | ||||
|     // Zero-out the gradient
 | ||||
|     g.setZero(); | ||||
|     Errors e; | ||||
|     BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { | ||||
|       e.push_back(-factor->getb()); | ||||
|     } | ||||
|     transposeMultiplyAdd(fg, 1.0, e, g); | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) { | ||||
|     Index i = 0 ; | ||||
|     BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { | ||||
|       r[i] = factor->getb(); | ||||
|       i++; | ||||
|     } | ||||
|     VectorValues Ax = VectorValues::SameStructure(r); | ||||
|     multiply(fg,x,Ax); | ||||
|     axpy(-1.0,Ax,r); | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) { | ||||
|     r.vector() = Vector::Zero(r.dim()); | ||||
|     Index i = 0; | ||||
|     BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { | ||||
|       SubVector &y = r[i]; | ||||
|       for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { | ||||
|         y += factor->getA(j) * x[*j]; | ||||
|       } | ||||
|       ++i; | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x) { | ||||
|     x.vector() = Vector::Zero(x.dim()); | ||||
|     Index i = 0; | ||||
|     BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { | ||||
|       for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { | ||||
|         x[*j] += factor->getA(j).transpose() * r[i]; | ||||
|       } | ||||
|       ++i; | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
| } | ||||
|  |  | |||
|  | @ -318,45 +318,5 @@ namespace gtsam { | |||
|     } | ||||
| 
 | ||||
|   }; // JacobianFactor
 | ||||
| 
 | ||||
|   /** return A*x */ | ||||
|   Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x); | ||||
| 
 | ||||
|   /* In-place version e <- A*x that overwrites e. */ | ||||
|   void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, Errors& e); | ||||
| 
 | ||||
|   /* In-place version e <- A*x that takes an iterator. */ | ||||
|   void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const Errors::iterator& e); | ||||
| 
 | ||||
|   /** x += alpha*A'*e */ | ||||
|   void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, const Errors& e, VectorValues& x); | ||||
| 
 | ||||
|   /**
 | ||||
|    * Compute the gradient of the energy function, | ||||
|    * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, | ||||
|    * centered around \f$ x = x_0 \f$. | ||||
|    * The gradient is \f$ A^T(Ax-b) \f$. | ||||
|    * @param fg The Jacobian factor graph $(A,b)$ | ||||
|    * @param x0 The center about which to compute the gradient | ||||
|    * @return The gradient as a VectorValues | ||||
|    */ | ||||
|   VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x0); | ||||
| 
 | ||||
|   /**
 | ||||
|    * Compute the gradient of the energy function, | ||||
|    * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, | ||||
|    * centered around zero. | ||||
|    * The gradient is \f$ A^T(Ax-b) \f$. | ||||
|    * @param fg The Jacobian factor graph $(A,b)$ | ||||
|    * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues | ||||
|    * @return The gradient as a VectorValues | ||||
|    */ | ||||
|   void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g); | ||||
| 
 | ||||
|   // matrix-vector operations
 | ||||
|   void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r); | ||||
|   void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r); | ||||
|   void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x); | ||||
| 
 | ||||
| } // gtsam
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -0,0 +1,108 @@ | |||
| /*
 | ||||
|  * JacobianFactorGraph.cpp | ||||
|  * | ||||
|  *  Created on: Jun 6, 2012 | ||||
|  *      Author: ydjian | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/linear/JacobianFactorGraph.h> | ||||
| #include <boost/foreach.hpp> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x) { | ||||
|   Errors e; | ||||
|   BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { | ||||
|     e.push_back((*Ai)*x); | ||||
|   } | ||||
|   return e; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, Errors& e) { | ||||
|   multiplyInPlace(fg,x,e.begin()); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const Errors::iterator& e) { | ||||
|   Errors::iterator ei = e; | ||||
|   BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { | ||||
|     *ei = (*Ai)*x; | ||||
|     ei++; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // x += alpha*A'*e
 | ||||
| void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, const Errors& e, VectorValues& x) { | ||||
|   // For each factor add the gradient contribution
 | ||||
|   Errors::const_iterator ei = e.begin(); | ||||
|   BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) { | ||||
|     Ai->transposeMultiplyAdd(alpha,*(ei++),x); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x0) { | ||||
|   // It is crucial for performance to make a zero-valued clone of x
 | ||||
|   VectorValues g = VectorValues::Zero(x0); | ||||
|   Errors e; | ||||
|   BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { | ||||
|     e.push_back(factor->error_vector(x0)); | ||||
|   } | ||||
|   transposeMultiplyAdd(fg, 1.0, e, g); | ||||
|   return g; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g) { | ||||
|   // Zero-out the gradient
 | ||||
|   g.setZero(); | ||||
|   Errors e; | ||||
|   BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { | ||||
|     e.push_back(-factor->getb()); | ||||
|   } | ||||
|   transposeMultiplyAdd(fg, 1.0, e, g); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) { | ||||
|   Index i = 0 ; | ||||
|   BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { | ||||
|     r[i] = factor->getb(); | ||||
|     i++; | ||||
|   } | ||||
|   VectorValues Ax = VectorValues::SameStructure(r); | ||||
|   multiply(fg,x,Ax); | ||||
|   axpy(-1.0,Ax,r); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r) { | ||||
|   r.vector() = Vector::Zero(r.dim()); | ||||
|   Index i = 0; | ||||
|   BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { | ||||
|     SubVector &y = r[i]; | ||||
|     for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { | ||||
|       y += factor->getA(j) * x[*j]; | ||||
|     } | ||||
|     ++i; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x) { | ||||
|   x.vector() = Vector::Zero(x.dim()); | ||||
|   Index i = 0; | ||||
|   BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) { | ||||
|     for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) { | ||||
|       x[*j] += factor->getA(j).transpose() * r[i]; | ||||
|     } | ||||
|     ++i; | ||||
|   } | ||||
| } | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
|  | @ -0,0 +1,70 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /*
 | ||||
|  * JacobianFactorGraph.cpp | ||||
|  * | ||||
|  *  Created on: Jun 6, 2012 | ||||
|  *      Author: ydjian | ||||
|  */ | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/linear/Errors.h> | ||||
| #include <gtsam/inference/FactorGraph.h> | ||||
| #include <gtsam/linear/JacobianFactor.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|   /**
 | ||||
|    * Linear Algebra Operations for the JacobianFactorGraph | ||||
|    */ | ||||
| 
 | ||||
|   /** return A*x */ | ||||
|   Errors operator*(const FactorGraph<JacobianFactor>& fg, const VectorValues& x); | ||||
| 
 | ||||
|   /** In-place version e <- A*x that overwrites e. */ | ||||
|   void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, Errors& e); | ||||
| 
 | ||||
|   /** In-place version e <- A*x that takes an iterator. */ | ||||
|   void multiplyInPlace(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const Errors::iterator& e); | ||||
| 
 | ||||
|   /** x += alpha*A'*e */ | ||||
|   void transposeMultiplyAdd(const FactorGraph<JacobianFactor>& fg, double alpha, const Errors& e, VectorValues& x); | ||||
| 
 | ||||
|   /**
 | ||||
|    * Compute the gradient of the energy function, | ||||
|    * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, | ||||
|    * centered around \f$ x = x_0 \f$. | ||||
|    * The gradient is \f$ A^T(Ax-b) \f$. | ||||
|    * @param fg The Jacobian factor graph $(A,b)$ | ||||
|    * @param x0 The center about which to compute the gradient | ||||
|    * @return The gradient as a VectorValues | ||||
|    */ | ||||
|   VectorValues gradient(const FactorGraph<JacobianFactor>& fg, const VectorValues& x0); | ||||
| 
 | ||||
|   /**
 | ||||
|    * Compute the gradient of the energy function, | ||||
|    * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, | ||||
|    * centered around zero. | ||||
|    * The gradient is \f$ A^T(Ax-b) \f$. | ||||
|    * @param fg The Jacobian factor graph $(A,b)$ | ||||
|    * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues | ||||
|    * @return The gradient as a VectorValues | ||||
|    */ | ||||
|   void gradientAtZero(const FactorGraph<JacobianFactor>& fg, VectorValues& g); | ||||
| 
 | ||||
|   /* matrix-vector operations */ | ||||
|   void residual(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r); | ||||
|   void multiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &x, VectorValues &r); | ||||
|   void transposeMultiply(const FactorGraph<JacobianFactor>& fg, const VectorValues &r, VectorValues &x); | ||||
| 
 | ||||
| } | ||||
|  | @ -13,7 +13,7 @@ | |||
| #include <gtsam/linear/GaussianConditional.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/GaussianBayesNet.h> | ||||
| #include <gtsam/linear/JacobianFactor.h> | ||||
| #include <gtsam/linear/JacobianFactorGraph.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <gtsam/inference/EliminationTree.h> | ||||
| #include <boost/foreach.hpp> | ||||
|  |  | |||
|  | @ -26,6 +26,7 @@ using namespace boost::assign; | |||
| #include <gtsam/linear/GaussianJunctionTree.h> | ||||
| #include <gtsam/inference/BayesTree-inl.h> | ||||
| #include <gtsam/linear/HessianFactor.h> | ||||
| #include <gtsam/linear/JacobianFactorGraph.h> | ||||
| 
 | ||||
| #include <gtsam/nonlinear/ISAM2.h> | ||||
| #include <gtsam/nonlinear/DoglegOptimizerImpl.h> | ||||
|  |  | |||
|  | @ -19,6 +19,7 @@ | |||
| #include <gtsam/nonlinear/Symbol.h> | ||||
| #include <gtsam/linear/GaussianBayesNet.h> | ||||
| #include <gtsam/linear/GaussianSequentialSolver.h> | ||||
| #include <gtsam/linear/JacobianFactorGraph.h> | ||||
| #include <gtsam/inference/SymbolicFactorGraph.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/base/Matrix.h> | ||||
|  |  | |||
|  | @ -10,6 +10,7 @@ | |||
| #include <gtsam/linear/GaussianBayesNet.h> | ||||
| #include <gtsam/linear/GaussianSequentialSolver.h> | ||||
| #include <gtsam/linear/GaussianBayesTree.h> | ||||
| #include <gtsam/linear/JacobianFactorGraph.h> | ||||
| #include <gtsam/nonlinear/ISAM2.h> | ||||
| #include <gtsam/slam/smallExample.h> | ||||
| #include <gtsam/slam/planarSLAM.h> | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue