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


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