add BlockJacobiPreconditioner class and unit test
							parent
							
								
									e8d3809917
								
							
						
					
					
						commit
						67398f0f13
					
				|  | @ -9,6 +9,7 @@ | |||
| //#include <gtsam/linear/CombinatorialPreconditioner.h>
 | ||||
| #include <gtsam/inference/FactorGraph-inst.h> | ||||
| //#include <gtsam/linear/FactorGraphUtil-inl.h>
 | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/PCGSolver.h> | ||||
| #include <gtsam/linear/Preconditioner.h> | ||||
| //#include <gtsam/linear/JacobianFactorGraph.h>
 | ||||
|  | @ -83,26 +84,137 @@ std::string PreconditionerParameters::verbosityTranslator(PreconditionerParamete | |||
|   else return "UNKNOWN"; | ||||
| } | ||||
| 
 | ||||
| ///***************************************************************************************/
 | ||||
| //void Preconditioner::replaceFactors(const JacobianFactorGraph &jfg, const double lambda)
 | ||||
| //{
 | ||||
| //  const Parameters &p = *parameters_;
 | ||||
| //
 | ||||
| //  if ( keyInfo_.size() == 0 ) {
 | ||||
| //    keyInfo_ = KeyInfo(jfg, *orderingNatural(jfg));
 | ||||
| //  }
 | ||||
| //
 | ||||
| //  if ( p.verbosity() >= Parameters::COMPLEXITY )
 | ||||
| //    cout << "Preconditioner::replaceFactors with a jfg of " << jfg.size() << " factors."<< endl;
 | ||||
| //}
 | ||||
| //
 | ||||
| /***************************************************************************************/ | ||||
| BlockJacobiPreconditioner::BlockJacobiPreconditioner() | ||||
|   : Base(), buffer_(0), bufferSize_(0), nnz_(0) {} | ||||
| 
 | ||||
| /***************************************************************************************/ | ||||
| BlockJacobiPreconditioner::~BlockJacobiPreconditioner() { clean(); } | ||||
| 
 | ||||
| /***************************************************************************************/ | ||||
| void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const { | ||||
| 
 | ||||
|   const size_t n = dims_.size(); | ||||
|   double *ptr = buffer_, *dst = x.data(); | ||||
| 
 | ||||
|   std::copy(y.data(), y.data() + y.rows(), x.data()); | ||||
| 
 | ||||
|   for ( size_t i = 0 ; i < n ; ++i ) { | ||||
|     const size_t d = dims_[i]; | ||||
|     const size_t sz = d*d; | ||||
| 
 | ||||
|     const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d); | ||||
|     Eigen::Map<Eigen::VectorXd> b(dst, d, 1); | ||||
|     R.triangularView<Eigen::Upper>().solveInPlace(b); | ||||
| 
 | ||||
|     dst += d; | ||||
|     ptr += sz; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /***************************************************************************************/ | ||||
| void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const { | ||||
|   const size_t n = dims_.size(); | ||||
|   double *ptr = buffer_, *dst = x.data(); | ||||
| 
 | ||||
|   std::copy(y.data(), y.data() + y.rows(), x.data()); | ||||
| 
 | ||||
|   for ( size_t i = 0 ; i < n ; ++i ) { | ||||
|     const size_t d = dims_[i]; | ||||
|     const size_t sz = d*d; | ||||
| 
 | ||||
|     const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d); | ||||
|     Eigen::Map<Eigen::VectorXd> b(dst, d, 1); | ||||
|     R.transpose().triangularView<Eigen::Upper>().solveInPlace(b); | ||||
| 
 | ||||
|     dst += d; | ||||
|     ptr += sz; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /***************************************************************************************/ | ||||
| void BlockJacobiPreconditioner::build( | ||||
|   const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda) | ||||
| { | ||||
|   const size_t n = keyInfo.size(); | ||||
|   dims_ = keyInfo.colSpec(); | ||||
| 
 | ||||
|   /* prepare the buffer of block diagonals */ | ||||
|   std::vector<Matrix> blocks; blocks.reserve(n); | ||||
| 
 | ||||
|   /* allocate memory for the factorization of block diagonals */ | ||||
|   size_t nnz = 0; | ||||
|   for ( size_t i = 0 ; i < n ; ++i ) { | ||||
|     const size_t dim = dims_[i]; | ||||
|     blocks.push_back(Matrix::Zero(dim, dim)); | ||||
|     // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2  ;
 | ||||
|     nnz += dim*dim; | ||||
|   } | ||||
| 
 | ||||
|   /* compute the block diagonal by scanning over the factors */ | ||||
|   BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { | ||||
|     if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) { | ||||
|       for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { | ||||
|         const KeyInfoEntry &entry =  keyInfo.find(*it)->second; | ||||
|         const Matrix &Ai = jf->getA(it); | ||||
|         blocks[entry.index()] += (Ai.transpose() * Ai); | ||||
|       } | ||||
|     } | ||||
|     else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) { | ||||
|       for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) { | ||||
|         const KeyInfoEntry &entry =  keyInfo.find(*it)->second; | ||||
|         const Matrix &Hii = hf->info(it, it).selfadjointView(); | ||||
|         blocks[entry.index()] += Hii; | ||||
|       } | ||||
|     } | ||||
|     else { | ||||
|       throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /* if necessary, allocating the memory for cacheing the factorization results */ | ||||
|   if ( nnz > bufferSize_ ) { | ||||
|     clean(); | ||||
|     buffer_ = new double [nnz]; | ||||
|     bufferSize_ = nnz; | ||||
|   } | ||||
|   nnz_ = nnz; | ||||
| 
 | ||||
|   /* factorizing the blocks respectively */ | ||||
|   double *ptr = buffer_; | ||||
|   for ( size_t i = 0 ; i < n ; ++i ) { | ||||
|     /* use eigen to decompose Di */ | ||||
|     const Matrix R = blocks[i].llt().matrixL().transpose(); | ||||
| 
 | ||||
|     /* store the data in the buffer */ | ||||
|     size_t sz = dims_[i]*dims_[i] ; | ||||
|     std::copy(R.data(), R.data() + sz, ptr); | ||||
| 
 | ||||
|     /* advance the pointer */ | ||||
|     ptr += sz; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /*****************************************************************************/ | ||||
| void BlockJacobiPreconditioner::clean() { | ||||
|   if ( buffer_ ) { | ||||
|     delete [] buffer_; | ||||
|     buffer_ = 0; | ||||
|     bufferSize_ = 0; | ||||
|     nnz_ = 0; | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /***************************************************************************************/ | ||||
| boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters) { | ||||
| 
 | ||||
|   DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters); | ||||
|   if ( dummy ) { | ||||
|   if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters) ) { | ||||
|     return boost::make_shared<DummyPreconditioner>(); | ||||
|   } | ||||
|   else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(parameters) ) { | ||||
|     return boost::make_shared<BlockJacobiPreconditioner>(); | ||||
|   } | ||||
| 
 | ||||
| 
 | ||||
| //  BlockJacobiPreconditioner::Parameters::shared_ptr jacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditioner::Parameters>(parameters);
 | ||||
| //  if ( jacobi ) {
 | ||||
|  |  | |||
|  | @ -18,7 +18,6 @@ namespace gtsam { | |||
| class GaussianFactorGraph; | ||||
| class KeyInfo; | ||||
| class VectorValues; | ||||
| //class Subgraph;
 | ||||
| 
 | ||||
| /* parameters for the preconditioner */ | ||||
| struct PreconditionerParameters { | ||||
|  | @ -75,15 +74,15 @@ public: | |||
| 
 | ||||
|   /* implement x = S^{-1} y */ | ||||
|   virtual void solve(const Vector& y, Vector &x) const = 0; | ||||
|   virtual void solve(const VectorValues& y, VectorValues &x) const = 0; | ||||
| //  virtual void solve(const VectorValues& y, VectorValues &x) const = 0;
 | ||||
| 
 | ||||
|   /* implement x = S^{-T} y */ | ||||
|   virtual void transposeSolve(const Vector& y, Vector& x) const = 0; | ||||
|   virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0; | ||||
| //  virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0;
 | ||||
| 
 | ||||
|   /* implement x = S^{-1} S^{-T} y */ | ||||
|   virtual void fullSolve(const Vector& y, Vector &x) const = 0; | ||||
|   virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0; | ||||
| //  /* implement x = S^{-1} S^{-T} y */
 | ||||
| //  virtual void fullSolve(const Vector& y, Vector &x) const = 0;
 | ||||
| //  virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0;
 | ||||
| 
 | ||||
|   /* build/factorize the preconditioner */ | ||||
|   virtual void build( | ||||
|  | @ -91,23 +90,6 @@ public: | |||
|     const KeyInfo &info, | ||||
|     const std::map<Key,Vector> &lambda | ||||
|     ) = 0; | ||||
| 
 | ||||
| //  /* complexity index */
 | ||||
| //  virtual size_t complexity() const = 0;
 | ||||
| //
 | ||||
| //  /* is the preconditioner dependent to data */
 | ||||
| //  virtual bool isStatic() const = 0;
 | ||||
| //
 | ||||
| //  /* is the preconditioner kind of spanning subgraph preconditioner */
 | ||||
| //  virtual bool isSubgraph() const = 0;
 | ||||
| //
 | ||||
| //  /* return A\b */
 | ||||
| //  virtual void xstar(Vector &result) const = 0 ;
 | ||||
| //
 | ||||
| //protected:
 | ||||
| //  Parameters::shared_ptr parameters_;
 | ||||
| //  KeyInfo keyInfo_;
 | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
| /*******************************************************************************************/ | ||||
|  | @ -131,30 +113,57 @@ public: | |||
| 
 | ||||
|   /* Computation Interfaces for raw vector */ | ||||
|   virtual void solve(const Vector& y, Vector &x) const { x = y; } | ||||
|   virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; } | ||||
| //  virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; }
 | ||||
| 
 | ||||
|   virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; } | ||||
|   virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; } | ||||
| //  virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; }
 | ||||
| 
 | ||||
|   virtual void fullSolve(const Vector& y, Vector &x) const { x = y; } | ||||
|   virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; } | ||||
| //  virtual void fullSolve(const Vector& y, Vector &x) const { x = y; }
 | ||||
| //  virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; }
 | ||||
| 
 | ||||
|   virtual void build( | ||||
|     const GaussianFactorGraph &gfg, | ||||
|     const KeyInfo &info, | ||||
|     const std::map<Key,Vector> &lambda | ||||
|     )  {} | ||||
| 
 | ||||
| //  virtual void replaceFactors(const JacobianFactorGraph &jfg, const double lambda = 0.0) {
 | ||||
| //    Base::replaceFactors(jfg,lambda);
 | ||||
| //  }
 | ||||
| //  virtual void buildPreconditioner() {}
 | ||||
| //  virtual size_t complexity() const { return 0; }
 | ||||
| //  virtual bool isStatic() const { return true; }
 | ||||
| //  virtual bool isSubgraph() const { return false; }
 | ||||
| //  virtual void xstar(Vector &result) const {}
 | ||||
| }; | ||||
| 
 | ||||
| /*******************************************************************************************/ | ||||
| struct BlockJacobiPreconditionerParameters : public PreconditionerParameters { | ||||
|   typedef PreconditionerParameters Base; | ||||
|   BlockJacobiPreconditionerParameters() : Base() {} | ||||
|   virtual ~BlockJacobiPreconditionerParameters() {} | ||||
| }; | ||||
| 
 | ||||
| /*******************************************************************************************/ | ||||
| class BlockJacobiPreconditioner : public Preconditioner { | ||||
| public: | ||||
|   typedef Preconditioner Base; | ||||
|   BlockJacobiPreconditioner() ; | ||||
|   virtual ~BlockJacobiPreconditioner() ; | ||||
| 
 | ||||
|   /* Computation Interfaces for raw vector */ | ||||
|   virtual void solve(const Vector& y, Vector &x) const; | ||||
|   virtual void transposeSolve(const Vector& y, Vector& x) const ; | ||||
| //  virtual void fullSolve(const Vector& y, Vector &x) const ;
 | ||||
| 
 | ||||
|   virtual void build( | ||||
|     const GaussianFactorGraph &gfg, | ||||
|     const KeyInfo &info, | ||||
|     const std::map<Key,Vector> &lambda | ||||
|     ) ; | ||||
| 
 | ||||
| protected: | ||||
| 
 | ||||
|   void clean() ; | ||||
| 
 | ||||
|   std::vector<size_t> dims_; | ||||
|   double *buffer_; | ||||
|   size_t bufferSize_; | ||||
|   size_t nnz_; | ||||
| }; | ||||
| 
 | ||||
| /*********************************************************************************************/ | ||||
| /* factory method to create preconditioners */ | ||||
| boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters); | ||||
| 
 | ||||
|  |  | |||
|  | @ -50,7 +50,47 @@ using symbol_shorthand::X; | |||
| using symbol_shorthand::L; | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( NonlinearOptimizer, optimization_method ) | ||||
| TEST( PCGSolver, llt ) { | ||||
|   Matrix R = (Matrix(3,3) << | ||||
|                 1., -1., -1., | ||||
|                 0.,  2., -1., | ||||
|                 0.,  0.,  1.); | ||||
|   Matrix AtA = R.transpose() * R; | ||||
| 
 | ||||
|   Vector Rvector = (Vector(9) << 1., -1., -1., | ||||
|                                  0.,  2., -1., | ||||
|                                  0.,  0.,  1.); | ||||
| //  Vector Rvector = (Vector(6) << 1., -1., -1.,
 | ||||
| //                                      2., -1.,
 | ||||
| //                                           1.);
 | ||||
| 
 | ||||
|   Vector b = (Vector(3) << 1., 2., 3.); | ||||
| 
 | ||||
|   Vector x = (Vector(3) << 6.5, 2.5, 3.) ; | ||||
| 
 | ||||
|   /* test cholesky */ | ||||
|   Matrix Rhat = AtA.llt().matrixL().transpose(); | ||||
|   EXPECT(assert_equal(R, Rhat, 1e-5)); | ||||
| 
 | ||||
|   /* test backward substitution */ | ||||
|   Vector xhat = Rhat.triangularView<Eigen::Upper>().solve(b); | ||||
|   EXPECT(assert_equal(x, xhat, 1e-5)); | ||||
| 
 | ||||
|   /* test in-place back substitution */ | ||||
|   xhat = b; | ||||
|   Rhat.triangularView<Eigen::Upper>().solveInPlace(xhat); | ||||
|   EXPECT(assert_equal(x, xhat, 1e-5)); | ||||
| 
 | ||||
|   /* test triangular matrix map */ | ||||
|   Eigen::Map<Eigen::MatrixXd> Radapter(Rvector.data(), 3, 3); | ||||
|   xhat = Radapter.transpose().triangularView<Eigen::Upper>().solve(b); | ||||
|   EXPECT(assert_equal(x, xhat, 1e-5)); | ||||
| 
 | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( PCGSolver, dummy ) | ||||
| { | ||||
|   LevenbergMarquardtParams paramsPCG; | ||||
|   paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; | ||||
|  | @ -69,6 +109,26 @@ TEST( NonlinearOptimizer, optimization_method ) | |||
|   DOUBLES_EQUAL(0,fg.error(actualPCG),tol); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( PCGSolver, blockjacobi ) | ||||
| { | ||||
|   LevenbergMarquardtParams paramsPCG; | ||||
|   paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; | ||||
|   PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>(); | ||||
|   pcg->preconditioner_ = boost::make_shared<BlockJacobiPreconditionerParameters>(); | ||||
|   paramsPCG.iterativeParams = pcg; | ||||
| 
 | ||||
|   NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); | ||||
| 
 | ||||
|   Point2 x0(10,10); | ||||
|   Values c0; | ||||
|   c0.insert(X(1), x0); | ||||
| 
 | ||||
|   Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize(); | ||||
| 
 | ||||
|   DOUBLES_EQUAL(0,fg.error(actualPCG),tol); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue