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 #include <nav2d_karto/csparse.h>
00039 
00040 #include <stdio.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         
00072   CSparse2d::CSparse2d()
00073   {
00074     A = NULL;
00075     AF = NULL;
00076 #ifdef SBA_CHOLMOD
00077     chA = NULL;
00078     chInited = false;
00079     Common.print=0;
00080 #endif
00081     asize = 0;
00082     csize = 0;
00083     nnz = 0;
00084     Bprev.resize(0);
00085   }
00086 
00087   CSparse2d::~CSparse2d()
00088   {
00089     if (A) cs_spfree(A);        // free any previous structure
00090     if (AF) cs_spfree(AF);      // free any previous structure
00091   }
00092 
00093 
00094         // 3x3 sparse jacobian blocks
00095         void CSparse2d::setupBlockStructure(int n, bool eraseit)
00096         {
00097                 // set up initial structure
00098                 diag.resize(n);
00099                 cols.resize(n);
00100                 if (eraseit)
00101                 {
00102                         for (int i=0; i<(int)cols.size(); i++)
00103                         {
00104                                 map<int,Matrix<double,3,3>, less<int>, 
00105                                 aligned_allocator< Matrix <double,3,3> > > &col = cols[i];
00106                                 col.clear();
00107                         }
00108                 }
00109                 asize = n;
00110                 csize = 3*n;
00111                 
00112                 if (eraseit)
00113                 {
00114                         // zero out entries
00115                         B.setZero(csize);
00116                         for (int i=0; i<(int)diag.size(); i++)
00117                         {
00118                                 diag[i].setZero();
00119                         }
00120                         for (int i=0; i<(int)cols.size(); i++)
00121                         {
00122                                 map<int,Matrix<double,3,3>, less<int>, 
00123                                 aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
00124                                 if (col.size() > 0)
00125                                 {
00126                                         map<int,Matrix<double,3,3>, less<int>, 
00127                                         aligned_allocator<Matrix<double,3,3> > >::iterator it;
00128                                         for (it = col.begin(); it != col.end(); it++)
00129                                         {
00130                                                 it->second.setZero();
00131                                         }
00132                                 }
00133                         }
00134                 }else // here we just resize B, saving the old parts
00135                 {
00136                         B.setZero(csize);
00137                         if (Bprev.size() > 0)
00138                         {
00139                                 B.head(Bprev.size()) = Bprev;
00140                         }
00141                 }
00142         }
00143 
00144 
00145   // add an off-diagonal block
00146   void CSparse2d::addOffdiagBlock(Matrix<double,3,3> &m, int ii, int jj)
00147   {
00148     // get column 
00149     map<int,Matrix<double,3,3>, less<int>, 
00150       aligned_allocator<Matrix<double,3,3> > > &col = cols[jj];
00151     // find current matrix
00152     map<int,Matrix<double,3,3>, less<int>, 
00153       aligned_allocator<Matrix<double,3,3> > >::iterator it;
00154     it = col.find(ii);
00155     if (it == col.end())        // didn't find it
00156       col.insert(pair<int,Matrix<double,3,3> >(ii,m));
00157     else                        // found it, add
00158       it->second += m;
00159   }
00160 
00161 
00162   void CSparse2d::incDiagBlocks(double lam)
00163   {
00164     for (int i=0; i<(int)diag.size(); i++)
00165       diag[i].diagonal() *= lam;
00166   }
00167 
00168 
00169   // set up CSparse2d structure; <init> true if first time
00170   // <diaginc> is the diagonal multiplier for LM
00171 
00172   // this version only sets upper triangular matrix,
00173   //   should set whole thing
00174 
00175   void CSparse2d::setupCSstructure(double diaginc, bool init)
00176   {
00177 #ifdef SBA_CHOLMOD
00178     if (useCholmod) {
00179       cholmod_start(&Common); // this is finished in doChol()
00180       Common.print = 0;
00181     }
00182 #endif
00183 
00184     // reserve space and set things up
00185     if (init || useCholmod)
00186       {
00187         if (A) cs_spfree(A);    // free any previous structure
00188 
00189         // count entries for cs allocation
00190         nnz = 6*asize;         // diagonal entries, just upper triangle
00191         for (int i=0; i<(int)cols.size(); i++)
00192           {
00193             map<int,Matrix<double,3,3>, less<int>, 
00194               aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
00195             nnz += 9 * col.size(); // 3x3 matrix
00196           }
00197 #ifdef SBA_CHOLMOD
00198         if (useCholmod)
00199           {
00200             //            cholmod_start(&Common); // this is finished in doChol()
00201             //            if (chA) 
00202             //              cholmod_free_sparse(&chA, &Common);
00203             chA = cholmod_allocate_sparse(csize,csize,nnz,true,true,1,CHOLMOD_REAL,&Common);
00204           }
00205         else
00206 #endif
00207           {
00208             A = cs_spalloc(csize,csize,nnz,1,0); // allocate sparse matrix
00209           }
00210         
00211         // now figure out the column pointers
00212         int colp = 0;           // index of where the column starts in Ai
00213         int *Ap, *Ai;
00214 #ifdef SBA_CHOLMOD
00215         if (useCholmod)
00216           {
00217             Ap = (int *)chA->p; // column pointer
00218             Ai = (int *)chA->i; // row indices
00219           }
00220         else
00221 #endif
00222           {
00223             Ap = A->p;          // column pointer
00224             Ai = A->i;          // row indices
00225           }
00226 
00227         for (int i=0; i<(int)cols.size(); i++)
00228           {
00229             // column i entries
00230             map<int,Matrix<double,3,3>, less<int>, 
00231               aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
00232 
00233             // do this for 3 columns
00234             for (int k=0; k<3; k++)
00235               {
00236                 *Ap++ = colp;
00237                 int row;
00238 
00239                 // iterate over the map
00240                 if (col.size() > 0)
00241                   {
00242                     // map iterator
00243                     map<int,Matrix<double,3,3>, less<int>, 
00244                       aligned_allocator<Matrix<double,3,3> > >::iterator it;
00245 
00246                     // iterate over block column entries
00247                     for (it = col.begin(); it != col.end(); it++)
00248                       {
00249                         row = 3*it->first; // which row we're at
00250                         for (int j=0; j<3; j++)
00251                           Ai[colp++] = row++;
00252                       }
00253                   }
00254 
00255                 // add in diagonal entries
00256                 row = 3*i;
00257                 for (int kk=0; kk<k+1; kk++)
00258                   Ai[colp++] = row++;
00259               }
00260            }        
00261           *Ap = nnz;       // last entry
00262        }
00263 
00264      // now put the entries in place
00265      int colp = 0;           // index of where the column starts in Ai
00266      double *Ax;
00267 #ifdef SBA_CHOLMOD
00268      if (useCholmod)
00269        Ax = (double *)chA->x;   // values
00270      else
00271 #endif
00272        Ax = A->x;               // values
00273 
00274      for (int i=0; i<(int)cols.size(); i++)
00275        {
00276          // column i entries
00277          map<int,Matrix<double,3,3>, less<int>, 
00278            aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
00279 
00280          // do this for 3 columns
00281          for (int k=0; k<3; k++)
00282            {
00283              // iterate over the map
00284              if (col.size() > 0)
00285                {
00286                  // map iterator
00287                  map<int,Matrix<double,3,3>, less<int>, 
00288                    aligned_allocator<Matrix<double,3,3> > >::iterator it;
00289 
00290                  // iterate over block column entries
00291                  for (it = col.begin(); it != col.end(); it++)
00292                    {
00293                      Matrix<double,3,3> &m = it->second;
00294                      for (int j=0; j<3; j++)
00295                        Ax[colp++] = m(j,k);
00296                    }
00297                } 
00298 
00299              // add in diagonal entries
00300              Matrix<double,3,3> &m = diag[i]; // diagonal block
00301              for (int kk=0; kk<k+1; kk++)
00302                Ax[colp++] = m(kk,k);
00303              Ax[colp-1] *= diaginc; // increment diagonal for LM
00304            }
00305        }      
00306 
00307      // make symmetric from upper diagonal
00308      // this could be more efficient if AT were eliminated and AF were fixed
00309      // oops - chol_sol only needs upper diag
00310   }
00311 
00312 
00313   // uncompress the compressed A into a dense Eigen matrix
00314   void CSparse2d::uncompress(MatrixXd &m)
00315   {
00316     if (!A) return;
00317     m.setZero(csize,csize);
00318     
00319     int *Ap = A->p;             // column pointer
00320     int *Ai = A->i;             // row indices
00321     double *Ax = A->x;          // values;
00322 
00323     for (int i=0; i<csize; i++)
00324       {
00325         int rbeg = Ap[i];
00326         int rend = Ap[i+1];
00327         if (rend > rbeg)
00328           for (int j=rbeg; j<rend; j++)
00329             m(Ai[j],i) = Ax[j];
00330       }
00331   }
00332 
00333   // solve in place, returns RHS B
00334   bool CSparse2d::doChol()
00335   {
00336 #ifdef SBA_CHOLMOD
00337     if (useCholmod)
00338       {
00339         cholmod_dense *x, b, *R, *R2;
00340         cholmod_factor *L ;
00341         double *Xx, *Rx, *bb;
00342         double one [2], minusone [2];
00343         one [0] = 1 ;
00344         one [1] = 0 ;
00345         minusone [0] = -1 ;
00346         minusone [1] = 0 ;
00347 
00348         //        cholmod_start (&Common) ;    // start it up ???
00349         cholmod_print_sparse (chA, (char *)"A", &Common) ; // print simple stats
00350         b.nrow = csize;
00351         b.ncol = 1;
00352         b.d = csize;                // leading dimension (???)
00353         b.nzmax = csize;
00354         b.xtype = CHOLMOD_REAL;
00355         b.dtype = CHOLMOD_DOUBLE;
00356         b.x = B.data();
00357         //cout << "CHOLMOD analyze..." << flush;
00358         L = cholmod_analyze (chA, &Common) ; // analyze 
00359         //cout << "factorize..." << flush;
00360         cholmod_factorize (chA, L, &Common) ; // factorize 
00361         //cout << "solve..." << flush;
00362         x = cholmod_solve (CHOLMOD_A, L, &b, &Common) ; // solve Ax=b
00363         //        cholmod_print_factor (L, (char *)"L", &Common) ;
00364 
00365         //cout << "refine" << endl;
00366         // one step of iterative refinement, cheap
00367         /* Ax=b was factorized and solved, R = B-A*X */
00368         R = cholmod_copy_dense (&b, &Common) ;
00369         cholmod_sdmult(chA, 0, minusone, one, x, R, &Common) ;
00370         /* R2 = A\(B-A*X) */
00371         R2 = cholmod_solve (CHOLMOD_A, L, R, &Common) ;
00372         /* compute X = X + A\(B-A*X) */
00373         Xx = (double *)x->x ;
00374         Rx = (double *)R2->x ;
00375         for (int i=0 ; i<csize ; i++)
00376         {
00377           Xx[i] = Xx[i] + Rx[i] ;
00378         }
00379         cholmod_free_dense (&R2, &Common) ;
00380         cholmod_free_dense (&R, &Common) ;
00381 
00382         bb = B.data();
00383         for (int i=0; i<csize; i++) // transfer answer
00384           *bb++ = *Xx++;
00385         cholmod_free_factor (&L, &Common) ; // free matrices 
00386         cholmod_free_dense (&x, &Common) ;
00387         cholmod_free_sparse(&chA, &Common);
00388         cholmod_finish (&Common) ;   // finish it ???
00389 
00390         return true;
00391       }
00392     else
00393 #endif
00394       {
00395 
00396         // using order 0 here (natural order); 
00397         // may be better to use "1" for large problems (AMD)
00398         int order = 0;
00399         if (csize > 100) order = 1;
00400         bool ok = (bool)cs_cholsol(order,A,B.data()); // do the CSparse2d thang
00401         return ok;
00402       }
00403   }
00404 
00405 
00406   // 
00407   // block jacobian PCG
00408   // max iterations <iter>, ending toleranace <tol>
00409   //
00410 /*
00411   int 
00412   CSparse2d::doBPCG(int iters, double tol, int sba_iter)
00413   {
00414     int n = B.rows();
00415     VectorXd x;
00416     x.setZero(n);
00417     bool abstol = false;
00418     if (sba_iter > 0) abstol = true;
00419     int ret;
00420     ret = bpcg.doBPCG2(iters, tol, diag, cols, x, B, abstol);
00421     B = x;                      // transfer result data
00422     return ret;
00423   }
00424 */
00425 
00426 #ifdef SBA_DSIF
00427   // solve in place, returns RHS B
00428   // PCG algorithm from SparseLib++, IML++
00429   int CSparse2d::doPCG(int iters)
00430   {
00431     // first convert sparse matrix to SparseLib++ format
00432     // do we need just upper triangular here???
00433 
00434     // add in lower triangular part
00435     cs *AT;
00436     AT = cs_transpose(A,1);
00437     cs_fkeep (AT, &dropdiag, NULL); // drop diagonal entries from AT 
00438     if (AF) cs_spfree(AF);      // free any previous structure
00439     AF = cs_add (A, AT, 1, 1);  // AF = A+AT
00440     cs_spfree (AT);
00441 
00442     // convert to SparseLib objects
00443     CompCol_Mat_double Ap(csize, csize, AF->nzmax, AF->x, AF->i, AF->p); 
00444     VECTOR_double b(B.data(),csize);   // Create rhs
00445     VECTOR_double x(csize, 0.0); // solution vectors
00446 
00447     // perform PCG
00448     int res;
00449     double tol = 1e-6;
00450     ICPreconditioner_double D(Ap); // Create diagonal preconditioner
00451     res = CG(Ap,x,b,D,iters,tol); // Solve system
00452 
00453     for (int i=0; i<csize; i++)
00454       B[i] = x[i];
00455 
00456     //    cout << "CG flag = " << res << endl;
00457     //    cout << "iterations performed: " << iters << endl;
00458     //    cout << "tolerance achieved  : " << tol << endl;
00459     //    cout << x << endl;
00460 
00461     return res;
00462   }
00463 #endif


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 03:53:08