csparse.cpp
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 // Interface to CSparse
00037 //
00038 
00039 #include <stdio.h>
00040 #include "sba/csparse.h"
00041 
00042 using namespace Eigen;
00043 
00044 #include <iostream>
00045 #include <iomanip>
00046 #include <fstream>
00047 #include <sys/time.h>
00048 #include <algorithm>
00049 
00050 using namespace std;
00051 
00052 
00053 //
00054 // Efficient interface to the CSparse sparse solver
00055 //
00056 // Basic idea is to store 6x6 blocks in a sparse structure,
00057 //   and when we're done forming the A matrix, transfer to
00058 //   a CSparse compressed structure.  The latter only has
00059 //   to be initialized once at the start of the nonlinear iterations.
00060 //
00061 
00062 //
00063 // CSparse holds its sparse NxN arrays in compressed column format:
00064 //
00065 //  col_ptr has N+1 entries, one for each column, and a final NNZ entry.
00066 //    each entry is the index in row_ptr of where the column starts.
00067 //  row_ind has NNZ entries, each one a row index for an element.
00068 //  vals    has NNZ entries corresponding to the row_ptr entries
00069 //  
00070 
00071 namespace sba
00072 {
00073 
00074   CSparse::CSparse()
00075   {
00076     A = NULL;
00077 #ifdef SBA_CHOLMOD
00078     chA = NULL;
00079     chInited = false;
00080 #endif
00081     useCholmod = false;
00082     asize = 0;
00083     csize = 0;
00084     nnz = 0;
00085   }
00086 
00087   CSparse::~CSparse()
00088   {
00089     if (A) cs_spfree(A);        // free any previous structure
00090   }
00091 
00092 
00093   // 6x6 sparse jacobian blocks
00094   void CSparse::setupBlockStructure(int n)
00095   {
00096     if (n > 0)                  // set up initial structure
00097       {
00098         diag.clear();
00099         diag.resize(n);
00100         cols.clear();
00101         cols.resize(n);
00102         asize = n;
00103         csize = 6*n;
00104       }
00105 
00106     // zero out entries
00107     B.setZero(csize);
00108     for (int i=0; i<(int)diag.size(); i++)
00109       diag[i].setZero();
00110     for (int i=0; i<(int)cols.size(); i++)
00111       {
00112         map<int,Matrix<double,6,6>, less<int>, 
00113           aligned_allocator<Matrix<double,6,6> > > &col = cols[i];
00114         if (col.size() > 0)
00115           {
00116             map<int,Matrix<double,6,6>, less<int>, 
00117               aligned_allocator<Matrix<double,6,6> > >::iterator it;
00118             for (it = col.begin(); it != col.end(); it++)
00119               it->second.setZero();
00120           }
00121       }
00122   }
00123 
00124   void CSparse::incDiagBlocks(double lam)
00125   {
00126     for (int i=0; i<(int)diag.size(); i++)
00127       diag[i].diagonal() *= lam;
00128   }
00129 
00130   // add an off-diagonal block
00131   void CSparse::addOffdiagBlock(Matrix<double,6,6> &m, int ii, int jj)
00132   {
00133     // get column 
00134     map<int,Matrix<double,6,6>, less<int>, 
00135       aligned_allocator<Matrix<double,6,6> > > &col = cols[jj];
00136     // find current matrix
00137     map<int,Matrix<double,6,6>, less<int>, 
00138       aligned_allocator<Matrix<double,6,6> > >::iterator it;
00139     it = col.find(ii);
00140     if (it == col.end())        // didn't find it
00141       col.insert(pair<int,Matrix<double,6,6> >(ii,m));
00142     else                        // found it, add
00143       it->second += m;
00144   }
00145 
00146 
00147   // set up CSparse structure; <init> true if first time
00148   // <diaginc> is the diagonal multiplier for LM
00149 
00150   // this version sets upper triangular matrix,
00151   void CSparse::setupCSstructure(double diaginc, bool init)
00152   {
00153 #ifdef SBA_CHOLMOD
00154     if (useCholmod) {
00155       cholmod_start(&Common); // this is finished in doChol()
00156       Common.print = 0;
00157     }
00158 #endif
00159 
00160     // reserve space and set things up
00161     if (init || useCholmod)
00162       {
00163         // count entries for cs allocation
00164         nnz = 21*asize;         // diagonal entries, just upper triangle
00165         for (int i=0; i<(int)cols.size(); i++)
00166           {
00167             map<int,Matrix<double,6,6>, less<int>, 
00168               aligned_allocator<Matrix<double,6,6> > > &col = cols[i];
00169             nnz += 36 * col.size(); // 6x6 matrix
00170           }
00171 
00172 #ifdef SBA_CHOLMOD
00173         if (useCholmod)
00174           {
00175             //            cholmod_start(&Common); // this is finished in doChol()
00176             //            if (chA) 
00177             //              cholmod_free_sparse(&chA, &Common);
00178             chA = cholmod_allocate_sparse(csize,csize,nnz,true,true,1,CHOLMOD_REAL,&Common);
00179           }
00180         else
00181 #endif
00182           {
00183             if (A) cs_spfree(A);    // free any previous structure
00184             A = cs_spalloc(csize,csize,nnz,1,0); // allocate sparse matrix
00185           }
00186 
00187         // now figure out the column pointers
00188         int colp = 0;           // index of where the column starts in Ai
00189         int *Ap, *Ai;
00190 #ifdef SBA_CHOLMOD
00191         if (useCholmod)
00192           {
00193             Ap = (int *)chA->p; // column pointer
00194             Ai = (int *)chA->i; // row indices
00195           }
00196         else
00197 #endif
00198           {
00199             Ap = A->p;          // column pointer
00200             Ai = A->i;          // row indices
00201           }
00202         for (int i=0; i<(int)cols.size(); i++)
00203           {
00204             // column i entries
00205             map<int,Matrix<double,6,6>, less<int>, 
00206               aligned_allocator<Matrix<double,6,6> > > &col = cols[i];
00207 
00208             // do this for 6 columns
00209             for (int k=0; k<6; k++)
00210               {
00211                 *Ap++ = colp;
00212                 int row;
00213 
00214                 // iterate over the map
00215                 if (col.size() > 0)
00216                   {
00217                     // map iterator
00218                     map<int,Matrix<double,6,6>, less<int>, 
00219                       aligned_allocator<Matrix<double,6,6> > >::iterator it;
00220 
00221                     // iterate over block column entries
00222                     for (it = col.begin(); it != col.end(); it++)
00223                       {
00224                         row = 6*it->first; // which row we're at
00225                         for (int j=0; j<6; j++)
00226                           Ai[colp++] = row++;
00227                       }
00228                   }
00229 
00230                 // add in diagonal entries
00231                 row = 6*i;
00232                 for (int kk=0; kk<k+1; kk++)
00233                   Ai[colp++] = row++;
00234               }
00235            }        
00236           *Ap = nnz;            // last entry
00237        }
00238 
00239 
00240 
00241      // now put the entries in place
00242      int colp = 0;              // index of where the column starts in Ai
00243      double *Ax;
00244 #ifdef SBA_CHOLMOD
00245      if (useCholmod)
00246        Ax = (double *)chA->x;   // values
00247      else
00248 #endif
00249        Ax = A->x;               // values
00250      for (int i=0; i<(int)cols.size(); i++)
00251        {
00252          // column i entries
00253          map<int,Matrix<double,6,6>, less<int>, 
00254            aligned_allocator<Matrix<double,6,6> > > &col = cols[i];
00255 
00256          // do this for 6 columns
00257          for (int k=0; k<6; k++)
00258            {
00259              // iterate over the map
00260              if (col.size() > 0)
00261                {
00262                  // map iterator
00263                  map<int,Matrix<double,6,6>, less<int>, 
00264                    aligned_allocator<Matrix<double,6,6> > >::iterator it;
00265 
00266                  // iterate over block column entries
00267                  for (it = col.begin(); it != col.end(); it++)
00268                    {
00269                      Matrix<double,6,6> &m = it->second;
00270                      for (int j=0; j<6; j++)
00271                        Ax[colp++] = m(j,k);
00272                    }
00273                } 
00274 
00275              // add in diagonal entries
00276              Matrix<double,6,6> &m = diag[i]; // diagonal block
00277              for (int kk=0; kk<k+1; kk++)
00278                Ax[colp++] = m(kk,k);
00279              Ax[colp-1] *= diaginc; // increment diagonal for LM
00280            }
00281        }      
00282 
00283   }
00284 
00285 
00286   // uncompress the compressed A into a dense Eigen matrix
00287   void CSparse::uncompress(MatrixXd &m)
00288   {
00289     if (!A) return;
00290     m.setZero(csize,csize);
00291     
00292     int *Ap = A->p;             // column pointer
00293     int *Ai = A->i;             // row indices
00294     double *Ax = A->x;          // values;
00295 
00296     for (int i=0; i<csize; i++)
00297       {
00298         int rbeg = Ap[i];
00299         int rend = Ap[i+1];
00300         if (rend > rbeg)
00301           for (int j=rbeg; j<rend; j++)
00302             m(Ai[j],i) = Ax[j];
00303       }
00304   }
00305 
00306   // solve in place, returns RHS B
00307   bool CSparse::doChol()
00308   {
00309 #ifdef SBA_CHOLMOD
00310     if (useCholmod)
00311       {
00312         cholmod_dense *x, b, *R, *R2;
00313         cholmod_factor *L ;
00314         double *Xx, *Rx, *bb;
00315         double one [2], minusone [2];
00316         one [0] = 1 ;
00317         one [1] = 0 ;
00318         minusone [0] = -1 ;
00319         minusone [1] = 0 ;
00320 
00321         //        cholmod_start (&Common) ;    // start it up ???
00322         cholmod_print_sparse (chA, (char *)"A", &Common) ; // print simple stats
00323         b.nrow = csize;
00324         b.ncol = 1;
00325         b.d = csize;                // leading dimension (???)
00326         b.nzmax = csize;
00327         b.xtype = CHOLMOD_REAL;
00328         b.dtype = CHOLMOD_DOUBLE;
00329         b.x = B.data();
00330         //cout << "CHOLMOD analyze..." << flush;
00331         L = cholmod_analyze (chA, &Common) ; // analyze 
00332         //cout << "factorize..." << flush;
00333         cholmod_factorize (chA, L, &Common) ; // factorize 
00334         //cout << "solve..." << flush;
00335         x = cholmod_solve (CHOLMOD_A, L, &b, &Common) ; // solve Ax=b
00336         //        cholmod_print_factor (L, (char *)"L", &Common) ;
00337 
00338         //cout << "refine" << endl;
00339         // one step of iterative refinement, cheap
00340         /* Ax=b was factorized and solved, R = B-A*X */
00341         R = cholmod_copy_dense (&b, &Common) ;
00342         cholmod_sdmult(chA, 0, minusone, one, x, R, &Common) ;
00343         /* R2 = A\(B-A*X) */
00344         R2 = cholmod_solve (CHOLMOD_A, L, R, &Common) ;
00345         /* compute X = X + A\(B-A*X) */
00346         Xx = (double *)x->x ;
00347         Rx = (double *)R2->x ;
00348         for (int i=0 ; i<csize ; i++)
00349         {
00350           Xx[i] = Xx[i] + Rx[i] ;
00351         }
00352         cholmod_free_dense (&R2, &Common) ;
00353         cholmod_free_dense (&R, &Common) ;
00354 
00355         bb = B.data();
00356         for (int i=0; i<csize; i++) // transfer answer
00357           *bb++ = *Xx++;
00358         cholmod_free_factor (&L, &Common) ; // free matrices 
00359         cholmod_free_dense (&x, &Common) ;
00360         cholmod_free_sparse(&chA, &Common);
00361         cholmod_finish (&Common) ;   // finish it ???
00362 
00363         return true;
00364       }
00365     else
00366 #endif
00367       {
00368         // using order 0 here (natural order); 
00369         // may be better to use "1" for large problems
00370         int order = 0;
00371         if (csize > 400) order = 1;
00372         bool ok = (bool)cs_cholsol(order,A,B.data()); // do the CSparse thang
00373         return ok;
00374       }
00375   }
00376 
00377 
00378   // 
00379   // block jacobian PCG
00380   // max iterations <iter>, ending toleranace <tol>
00381   //
00382 
00383   int 
00384   CSparse::doBPCG(int iters, double tol, int sba_iter)
00385   {
00386     int n = B.rows();
00387     VectorXd x;
00388     x.setZero(n);
00389     bool abstol = false;
00390     if (sba_iter > 0) abstol = true;
00391     int ret;
00392     ret = bpcg.doBPCG2(iters, tol, diag, cols, x, B, abstol);
00393     B = x;                      // transfer result data
00394     return ret;
00395   }
00396 
00397 
00398   //
00399   // 2d version
00400   //
00401 
00402   CSparse2d::CSparse2d()
00403   {
00404     A = NULL;
00405     AF = NULL;
00406 #ifdef SBA_CHOLMOD
00407     chA = NULL;
00408     chInited = false;
00409     Common.print=0;
00410 #endif
00411     asize = 0;
00412     csize = 0;
00413     nnz = 0;
00414     Bprev.resize(0);
00415   }
00416 
00417   CSparse2d::~CSparse2d()
00418   {
00419     if (A) cs_spfree(A);        // free any previous structure
00420     if (AF) cs_spfree(AF);      // free any previous structure
00421   }
00422 
00423 
00424   // 3x3 sparse jacobian blocks
00425   void CSparse2d::setupBlockStructure(int n, bool eraseit)
00426   {
00427     if (n > 0)                  // set up initial structure
00428       {
00429         diag.resize(n);
00430         cols.resize(n);
00431         if (eraseit)
00432           for (int i=0; i<(int)cols.size(); i++)
00433             {
00434               map<int,Matrix<double,3,3>, less<int>, 
00435                 aligned_allocator< Matrix <double,3,3> > > &col = cols[i];
00436               col.clear();
00437             }
00438         asize = n;
00439         csize = 3*n;
00440       }
00441 
00442     if (eraseit)
00443       {
00444         // zero out entries
00445         B.setZero(csize);
00446         for (int i=0; i<(int)diag.size(); i++)
00447           diag[i].setZero();
00448         for (int i=0; i<(int)cols.size(); i++)
00449           {
00450             map<int,Matrix<double,3,3>, less<int>, 
00451               aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
00452             if (col.size() > 0)
00453               {
00454                 map<int,Matrix<double,3,3>, less<int>, 
00455                   aligned_allocator<Matrix<double,3,3> > >::iterator it;
00456                 for (it = col.begin(); it != col.end(); it++)
00457                   it->second.setZero();
00458               }
00459           }
00460       }
00461 
00462     else                        // here we just resize B, saving the old parts
00463       {
00464         B.setZero(csize);
00465         if (Bprev.size() > 0)
00466           B.head(Bprev.size()) = Bprev;
00467       }
00468   }
00469 
00470 
00471   // add an off-diagonal block
00472   void CSparse2d::addOffdiagBlock(Matrix<double,3,3> &m, int ii, int jj)
00473   {
00474     // get column 
00475     map<int,Matrix<double,3,3>, less<int>, 
00476       aligned_allocator<Matrix<double,3,3> > > &col = cols[jj];
00477     // find current matrix
00478     map<int,Matrix<double,3,3>, less<int>, 
00479       aligned_allocator<Matrix<double,3,3> > >::iterator it;
00480     it = col.find(ii);
00481     if (it == col.end())        // didn't find it
00482       col.insert(pair<int,Matrix<double,3,3> >(ii,m));
00483     else                        // found it, add
00484       it->second += m;
00485   }
00486 
00487 
00488   void CSparse2d::incDiagBlocks(double lam)
00489   {
00490     for (int i=0; i<(int)diag.size(); i++)
00491       diag[i].diagonal() *= lam;
00492   }
00493 
00494 
00495   // set up CSparse2d structure; <init> true if first time
00496   // <diaginc> is the diagonal multiplier for LM
00497 
00498   // this version only sets upper triangular matrix,
00499   //   should set whole thing
00500 
00501   void CSparse2d::setupCSstructure(double diaginc, bool init)
00502   {
00503 #ifdef SBA_CHOLMOD
00504     if (useCholmod) {
00505       cholmod_start(&Common); // this is finished in doChol()
00506       Common.print = 0;
00507     }
00508 #endif
00509 
00510     // reserve space and set things up
00511     if (init || useCholmod)
00512       {
00513         if (A) cs_spfree(A);    // free any previous structure
00514 
00515         // count entries for cs allocation
00516         nnz = 6*asize;         // diagonal entries, just upper triangle
00517         for (int i=0; i<(int)cols.size(); i++)
00518           {
00519             map<int,Matrix<double,3,3>, less<int>, 
00520               aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
00521             nnz += 9 * col.size(); // 3x3 matrix
00522           }
00523 #ifdef SBA_CHOLMOD
00524         if (useCholmod)
00525           {
00526             //            cholmod_start(&Common); // this is finished in doChol()
00527             //            if (chA) 
00528             //              cholmod_free_sparse(&chA, &Common);
00529             chA = cholmod_allocate_sparse(csize,csize,nnz,true,true,1,CHOLMOD_REAL,&Common);
00530           }
00531         else
00532 #endif
00533           {
00534             A = cs_spalloc(csize,csize,nnz,1,0); // allocate sparse matrix
00535           }
00536         
00537         // now figure out the column pointers
00538         int colp = 0;           // index of where the column starts in Ai
00539         int *Ap, *Ai;
00540 #ifdef SBA_CHOLMOD
00541         if (useCholmod)
00542           {
00543             Ap = (int *)chA->p; // column pointer
00544             Ai = (int *)chA->i; // row indices
00545           }
00546         else
00547 #endif
00548           {
00549             Ap = A->p;          // column pointer
00550             Ai = A->i;          // row indices
00551           }
00552 
00553         for (int i=0; i<(int)cols.size(); i++)
00554           {
00555             // column i entries
00556             map<int,Matrix<double,3,3>, less<int>, 
00557               aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
00558 
00559             // do this for 3 columns
00560             for (int k=0; k<3; k++)
00561               {
00562                 *Ap++ = colp;
00563                 int row;
00564 
00565                 // iterate over the map
00566                 if (col.size() > 0)
00567                   {
00568                     // map iterator
00569                     map<int,Matrix<double,3,3>, less<int>, 
00570                       aligned_allocator<Matrix<double,3,3> > >::iterator it;
00571 
00572                     // iterate over block column entries
00573                     for (it = col.begin(); it != col.end(); it++)
00574                       {
00575                         row = 3*it->first; // which row we're at
00576                         for (int j=0; j<3; j++)
00577                           Ai[colp++] = row++;
00578                       }
00579                   }
00580 
00581                 // add in diagonal entries
00582                 row = 3*i;
00583                 for (int kk=0; kk<k+1; kk++)
00584                   Ai[colp++] = row++;
00585               }
00586            }        
00587           *Ap = nnz;       // last entry
00588        }
00589 
00590      // now put the entries in place
00591      int colp = 0;           // index of where the column starts in Ai
00592      double *Ax;
00593 #ifdef SBA_CHOLMOD
00594      if (useCholmod)
00595        Ax = (double *)chA->x;   // values
00596      else
00597 #endif
00598        Ax = A->x;               // values
00599 
00600      for (int i=0; i<(int)cols.size(); i++)
00601        {
00602          // column i entries
00603          map<int,Matrix<double,3,3>, less<int>, 
00604            aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
00605 
00606          // do this for 3 columns
00607          for (int k=0; k<3; k++)
00608            {
00609              // iterate over the map
00610              if (col.size() > 0)
00611                {
00612                  // map iterator
00613                  map<int,Matrix<double,3,3>, less<int>, 
00614                    aligned_allocator<Matrix<double,3,3> > >::iterator it;
00615 
00616                  // iterate over block column entries
00617                  for (it = col.begin(); it != col.end(); it++)
00618                    {
00619                      Matrix<double,3,3> &m = it->second;
00620                      for (int j=0; j<3; j++)
00621                        Ax[colp++] = m(j,k);
00622                    }
00623                } 
00624 
00625              // add in diagonal entries
00626              Matrix<double,3,3> &m = diag[i]; // diagonal block
00627              for (int kk=0; kk<k+1; kk++)
00628                Ax[colp++] = m(kk,k);
00629              Ax[colp-1] *= diaginc; // increment diagonal for LM
00630            }
00631        }      
00632 
00633      // make symmetric from upper diagonal
00634      // this could be more efficient if AT were eliminated and AF were fixed
00635      // oops - chol_sol only needs upper diag
00636   }
00637 
00638 
00639   // uncompress the compressed A into a dense Eigen matrix
00640   void CSparse2d::uncompress(MatrixXd &m)
00641   {
00642     if (!A) return;
00643     m.setZero(csize,csize);
00644     
00645     int *Ap = A->p;             // column pointer
00646     int *Ai = A->i;             // row indices
00647     double *Ax = A->x;          // values;
00648 
00649     for (int i=0; i<csize; i++)
00650       {
00651         int rbeg = Ap[i];
00652         int rend = Ap[i+1];
00653         if (rend > rbeg)
00654           for (int j=rbeg; j<rend; j++)
00655             m(Ai[j],i) = Ax[j];
00656       }
00657   }
00658 
00659   // solve in place, returns RHS B
00660   bool CSparse2d::doChol()
00661   {
00662 #ifdef SBA_CHOLMOD
00663     if (useCholmod)
00664       {
00665         cholmod_dense *x, b, *R, *R2;
00666         cholmod_factor *L ;
00667         double *Xx, *Rx, *bb;
00668         double one [2], minusone [2];
00669         one [0] = 1 ;
00670         one [1] = 0 ;
00671         minusone [0] = -1 ;
00672         minusone [1] = 0 ;
00673 
00674         //        cholmod_start (&Common) ;    // start it up ???
00675         cholmod_print_sparse (chA, (char *)"A", &Common) ; // print simple stats
00676         b.nrow = csize;
00677         b.ncol = 1;
00678         b.d = csize;                // leading dimension (???)
00679         b.nzmax = csize;
00680         b.xtype = CHOLMOD_REAL;
00681         b.dtype = CHOLMOD_DOUBLE;
00682         b.x = B.data();
00683         //cout << "CHOLMOD analyze..." << flush;
00684         L = cholmod_analyze (chA, &Common) ; // analyze 
00685         //cout << "factorize..." << flush;
00686         cholmod_factorize (chA, L, &Common) ; // factorize 
00687         //cout << "solve..." << flush;
00688         x = cholmod_solve (CHOLMOD_A, L, &b, &Common) ; // solve Ax=b
00689         //        cholmod_print_factor (L, (char *)"L", &Common) ;
00690 
00691         //cout << "refine" << endl;
00692         // one step of iterative refinement, cheap
00693         /* Ax=b was factorized and solved, R = B-A*X */
00694         R = cholmod_copy_dense (&b, &Common) ;
00695         cholmod_sdmult(chA, 0, minusone, one, x, R, &Common) ;
00696         /* R2 = A\(B-A*X) */
00697         R2 = cholmod_solve (CHOLMOD_A, L, R, &Common) ;
00698         /* compute X = X + A\(B-A*X) */
00699         Xx = (double *)x->x ;
00700         Rx = (double *)R2->x ;
00701         for (int i=0 ; i<csize ; i++)
00702         {
00703           Xx[i] = Xx[i] + Rx[i] ;
00704         }
00705         cholmod_free_dense (&R2, &Common) ;
00706         cholmod_free_dense (&R, &Common) ;
00707 
00708         bb = B.data();
00709         for (int i=0; i<csize; i++) // transfer answer
00710           *bb++ = *Xx++;
00711         cholmod_free_factor (&L, &Common) ; // free matrices 
00712         cholmod_free_dense (&x, &Common) ;
00713         cholmod_free_sparse(&chA, &Common);
00714         cholmod_finish (&Common) ;   // finish it ???
00715 
00716         return true;
00717       }
00718     else
00719 #endif
00720       {
00721 
00722         // using order 0 here (natural order); 
00723         // may be better to use "1" for large problems (AMD)
00724         int order = 0;
00725         if (csize > 100) order = 1;
00726         bool ok = (bool)cs_cholsol(order,A,B.data()); // do the CSparse2d thang
00727         return ok;
00728       }
00729   }
00730 
00731 
00732   // 
00733   // block jacobian PCG
00734   // max iterations <iter>, ending toleranace <tol>
00735   //
00736 
00737   int 
00738   CSparse2d::doBPCG(int iters, double tol, int sba_iter)
00739   {
00740     int n = B.rows();
00741     VectorXd x;
00742     x.setZero(n);
00743     bool abstol = false;
00744     if (sba_iter > 0) abstol = true;
00745     int ret;
00746     ret = bpcg.doBPCG2(iters, tol, diag, cols, x, B, abstol);
00747     B = x;                      // transfer result data
00748     return ret;
00749   }
00750 
00751 
00752 #ifdef SBA_DSIF
00753   // solve in place, returns RHS B
00754   // PCG algorithm from SparseLib++, IML++
00755   int CSparse2d::doPCG(int iters)
00756   {
00757     // first convert sparse matrix to SparseLib++ format
00758     // do we need just upper triangular here???
00759 
00760     // add in lower triangular part
00761     cs *AT;
00762     AT = cs_transpose(A,1);
00763     cs_fkeep (AT, &dropdiag, NULL); // drop diagonal entries from AT 
00764     if (AF) cs_spfree(AF);      // free any previous structure
00765     AF = cs_add (A, AT, 1, 1);  // AF = A+AT
00766     cs_spfree (AT);
00767 
00768     // convert to SparseLib objects
00769     CompCol_Mat_double Ap(csize, csize, AF->nzmax, AF->x, AF->i, AF->p); 
00770     VECTOR_double b(B.data(),csize);   // Create rhs
00771     VECTOR_double x(csize, 0.0); // solution vectors
00772 
00773     // perform PCG
00774     int res;
00775     double tol = 1e-6;
00776     ICPreconditioner_double D(Ap); // Create diagonal preconditioner
00777     res = CG(Ap,x,b,D,iters,tol); // Solve system
00778 
00779     for (int i=0; i<csize; i++)
00780       B[i] = x[i];
00781 
00782     //    cout << "CG flag = " << res << endl;
00783     //    cout << "iterations performed: " << iters << endl;
00784     //    cout << "tolerance achieved  : " << tol << endl;
00785     //    cout << x << endl;
00786 
00787     return res;
00788   }
00789 #endif
00790 
00791 
00792 } // end namespace sba


sba
Author(s): Kurt Konolige, Helen Oleynikova
autogenerated on Thu Jan 2 2014 12:12:08