bpcg.h
Go to the documentation of this file.
00001  /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 //
00036 // block preconditioned conjugate gradient
00037 // 6x6 blocks at present, should be templatized
00038 //
00039 
00040 #ifndef _BPCG_H_
00041 #define _BPCG_H_
00042 
00043 #ifndef EIGEN_USE_NEW_STDVECTOR
00044 #define EIGEN_USE_NEW_STDVECTOR
00045 #endif // EIGEN_USE_NEW_STDVECTOR
00046 
00047 #include <vector>
00048 #include <map>
00049 #include <Eigen/Core>
00050 #include <Eigen/Geometry>
00051 #include <Eigen/LU>
00052 #include <Eigen/StdVector>
00053 
00054 using namespace Eigen;
00055 using namespace std;
00056 
00057 //typedef Matrix<double,6,1> Vector6d;
00058 //typedef Vector6d::AlignedMapType AV6d;
00059 
00060 namespace sba
00061 {
00063 
00064   template <int N>
00065     class jacobiBPCG 
00066     {
00067     public:
00068       jacobiBPCG() { residual = 0.0; };
00069       int doBPCG(int iters, double tol,
00070                  vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00071                  vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
00072                  VectorXd &x,
00073                  VectorXd &b,
00074                  bool abstol = false,
00075                  bool verbose = false
00076                  );
00077 
00078       // uses internal linear storage for Hessian
00079       int doBPCG2(int iters, double tol,
00080                  vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00081                  vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
00082                  VectorXd &x,
00083                  VectorXd &b,
00084                  bool abstol = false,
00085                  bool verbose = false
00086                  );
00087 
00088       double residual;
00089 
00090     private:
00091       void mMV(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00092                vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
00093                const VectorXd &vin,
00094                VectorXd &vout);
00095  
00096       // uses internal linear storage
00097       void mMV2(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00098                 const VectorXd &vin,
00099                 VectorXd &vout);
00100  
00101       void mD(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00102               VectorXd &vin,
00103               VectorXd &vout);
00104 
00105       vector<int> vcind, vrind;
00106       vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > vcols;
00107     };
00108 
00109   template <int N>
00110     void jacobiBPCG<N>::mMV(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00111                         vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
00112                         const VectorXd &vin,
00113                         VectorXd &vout)
00114     {
00115     // loop over all entries
00116       if (cols.size() > 0)
00117         for (int i=0; i<(int)cols.size(); i++)
00118           {
00119             vout.segment<N>(i*N) = diag[i]*vin.segment<N>(i*N); // only works with cols ordering
00120 
00121             map<int,Matrix<double,N,N>, less<int>, 
00122               aligned_allocator<Matrix<double,N,N> > > &col = cols[i];
00123             if (col.size() > 0)
00124               {
00125                 typename map<int,Matrix<double,N,N>, less<int>, // need "typename" here, barf
00126                   aligned_allocator<Matrix<double,N,N > > >::iterator it;
00127                 for (it = col.begin(); it != col.end(); it++)
00128                   {
00129                     int ri = (*it).first; // get row index
00130                     const Matrix<double,N,N> &M = (*it).second; // matrix
00131                     vout.segment<N>(i*N)  += M.transpose()*vin.segment<N>(ri*N);
00132                     vout.segment<N>(ri*N) += M*vin.segment<N>(i*N);
00133                   }
00134               }
00135           }
00136     }
00137 
00138   //
00139   // matrix-vector multiply with linear storage
00140   //
00141 
00142   template <int N>
00143     void jacobiBPCG<N>::mMV2(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00144                              const VectorXd &vin,
00145                              VectorXd &vout)
00146     {
00147       // linear storage for matrices
00148       // loop over off-diag entries
00149       if (diag.size() > 0)
00150         for (int i=0; i<(int)diag.size(); i++)
00151           vout.segment<N>(i*N) = diag[i]*vin.segment<N>(i*N); // only works with cols ordering
00152 
00153       for (int i=0; i<(int)vcind.size(); i++)
00154         {
00155           int ri = vrind[i];
00156           int ii = vcind[i];
00157           const Matrix<double,N,N> &M = vcols[i];
00158           vout.segment<N>(ri*N) += M*vin.segment<N>(ii*N);
00159           vout.segment<N>(ii*N) += M.transpose()*vin.segment<N>(ri*N);
00160         }
00161     }
00162 
00163 
00164 
00165 
00166   template <int N>
00167     void jacobiBPCG<N>::mD(vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00168             VectorXd &vin,
00169             VectorXd &vout)
00170     {
00171       // loop over diag entries
00172       for (int i=0; i<(int)diag.size(); i++)
00173         vout.segment<N>(i*N) = diag[i]*vin.segment<N>(i*N);
00174     }
00175 
00176 
00177   template <int N>
00178     int jacobiBPCG<N>::doBPCG(int iters, double tol,
00179             vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00180             vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
00181             VectorXd &x,
00182             VectorXd &b,
00183             bool abstol,
00184             bool verbose)
00185     {
00186       // set up local vars
00187       VectorXd r,d,q,s;
00188       int n = diag.size();
00189       int n6 = n*N;
00190       r.setZero(n6);
00191       d.setZero(n6);
00192       q.setZero(n6);
00193       s.setZero(n6);
00194 
00195       // set up Jacobi preconditioner
00196       vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > J;
00197       J.resize(n);
00198       for (int i=0; i<n; i++)
00199         J[i] = diag[i].inverse();
00200 
00201       int i;
00202       r = b;
00203       mD(J,r,d);
00204       double dn = r.dot(d);
00205       double d0 = tol*dn;
00206       if (abstol)               // change tolerances
00207         {
00208           if (residual > d0) d0 = residual;
00209         }
00210 
00211       for (i=0; i<iters; i++)
00212         {
00213           if (verbose && 0)
00214             cout << "[BPCG] residual[" << i << "]: " << dn << " < " << d0 << endl;
00215           if (dn < d0) break;   // done
00216           mMV(diag,cols,d,q);
00217           double a = dn / d.dot(q);
00218           x += a*d;
00219           // TODO: reset residual here every 50 iterations
00220           r -= a*q;
00221           mD(J,r,s);
00222           double dold = dn;
00223           dn = r.dot(s);
00224           double ba = dn / dold;
00225           d = s + ba*d;
00226         }
00227 
00228   
00229       if (verbose)
00230         cout << "[BPCG] residual[" << i << "]: " << dn << endl;
00231       residual = dn/2.0;
00232       return i;
00233     }
00234 
00235 
00236   // uses internal linear storage for Hessian
00237   template <int N>
00238     int jacobiBPCG<N>::doBPCG2(int iters, double tol,
00239             vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > &diag,
00240             vector< map<int,Matrix<double,N,N>, less<int>, aligned_allocator<Matrix<double,N,N> > > > &cols,
00241             VectorXd &x,
00242             VectorXd &b,
00243             bool abstol,
00244             bool verbose)
00245     {
00246       // set up local vars
00247       VectorXd r,d,q,s;
00248       int n = diag.size();
00249       int n6 = n*N;
00250       r.setZero(n6);
00251       d.setZero(n6);
00252       q.setZero(n6);
00253       s.setZero(n6);
00254 
00255       vcind.clear();
00256       vrind.clear();
00257       vcols.clear();
00258 
00259       // set up alternate rep for sparse matrix
00260       for (int i=0; i<(int)cols.size(); i++)
00261         {
00262           map<int,Matrix<double,N,N>, less<int>, 
00263             aligned_allocator<Matrix<double,N,N> > > &col = cols[i];
00264           if (col.size() > 0)
00265             {
00266               typename map<int,Matrix<double,N,N>, less<int>, 
00267                 aligned_allocator<Matrix<double,N,N> > >::iterator it;
00268               for (it = col.begin(); it != col.end(); it++)
00269                 {
00270                   int ri = (*it).first; // get row index
00271                   vrind.push_back(ri);
00272                   vcind.push_back(i);
00273                   vcols.push_back((*it).second);
00274                 }
00275             }
00276         }
00277 
00278       // set up Jacobi preconditioner
00279       vector< Matrix<double,N,N>, aligned_allocator<Matrix<double,N,N> > > J;
00280       J.resize(n);
00281       for (int i=0; i<n; i++)
00282         J[i] = diag[i].inverse();
00283 
00284       int i;
00285       r = b;
00286       mD(J,r,d);
00287       double dn = r.dot(d);
00288       double d0 = tol*dn;
00289       if (abstol)               // change tolerances
00290         {
00291           if (residual > d0) d0 = residual;
00292         }
00293 
00294       for (i=0; i<iters; i++)
00295         {
00296           if (verbose && 0)
00297             cout << "[BPCG] residual[" << i << "]: " << dn << " < " << d0 << endl;
00298           if (dn < d0) break;   // done
00299           mMV2(diag,d,q);
00300           double a = dn / d.dot(q);
00301           x += a*d;
00302           // TODO: reset residual here every 50 iterations
00303           r -= a*q;
00304           mD(J,r,s);
00305           double dold = dn;
00306           dn = r.dot(s);
00307           double ba = dn / dold;
00308           d = s + ba*d;
00309         }
00310 
00311   
00312       if (verbose)
00313         cout << "[BPCG] residual[" << i << "]: " << dn << endl;
00314       residual = dn/2.0;
00315       return i;
00316     }
00317 
00318 
00319 #if 0
00320 //
00321 // matrix multiply of compressed column storage + diagonal blocks by a vector
00322 //
00323 
00324 void
00325 mMV(vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > &diag,
00326     vector< map<int,Matrix<double,6,6>, less<int>, aligned_allocator<Matrix<double,6,6> > > > &cols,
00327     const VectorXd &vin,
00328     VectorXd &vout);
00329 
00330 //
00331 // jacobi-preconditioned block conjugate gradient
00332 // returns number of iterations
00333 
00334 int
00335 bpcg_jacobi(int iters, double tol,
00336             vector< Matrix<double,6,6>, aligned_allocator<Matrix<double,6,6> > > &diag,
00337             vector< map<int,Matrix<double,6,6>, less<int>, aligned_allocator<Matrix<double,6,6> > > > &cols,
00338             VectorXd &x,
00339             VectorXd &b,
00340             bool abstol = false,
00341             bool verbose = false
00342          );
00343 
00344 int
00345 bpcg_jacobi_dense(int iters, double tol,
00346                   MatrixXd &M,
00347                   VectorXd &x,
00348                   VectorXd &b);
00349 
00350 //
00351 // jacobi-preconditioned block conjugate gradient
00352 // returns number of iterations
00353 
00354 int
00355 bpcg_jacobi3(int iters, double tol,
00356             vector< Matrix<double,3,3>, aligned_allocator<Matrix<double,3,3> > > &diag,
00357             vector< map<int,Matrix<double,3,3>, less<int>, aligned_allocator<Matrix<double,3,3> > > > &cols,
00358             VectorXd &x,
00359             VectorXd &b,
00360             bool abstol = false,
00361             bool verbose = false
00362          );
00363 
00364 int
00365 bpcg_jacobi_dense3(int iters, double tol,
00366                   MatrixXd &M,
00367                   VectorXd &x,
00368                   VectorXd &b);
00369 
00370 #endif
00371 
00372 }  // end namespace sba
00373 
00374 #endif // BPCG_H


bpcg
Author(s): Kurt Konolige
autogenerated on Thu Jan 2 2014 12:12:00