csparse.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 //
36 // Interface to CSparse
37 //
38 #include <nav2d_karto/csparse.h>
39 
40 #include <stdio.h>
41 
42 using namespace Eigen;
43 
44 #include <iostream>
45 #include <iomanip>
46 #include <fstream>
47 #include <sys/time.h>
48 #include <algorithm>
49 
50 using namespace std;
51 
52 
53 //
54 // Efficient interface to the CSparse sparse solver
55 //
56 // Basic idea is to store 6x6 blocks in a sparse structure,
57 // and when we're done forming the A matrix, transfer to
58 // a CSparse compressed structure. The latter only has
59 // to be initialized once at the start of the nonlinear iterations.
60 //
61 
62 //
63 // CSparse holds its sparse NxN arrays in compressed column format:
64 //
65 // col_ptr has N+1 entries, one for each column, and a final NNZ entry.
66 // each entry is the index in row_ptr of where the column starts.
67 // row_ind has NNZ entries, each one a row index for an element.
68 // vals has NNZ entries corresponding to the row_ptr entries
69 //
70 
71 
73  {
74  A = NULL;
75  AF = NULL;
76 #ifdef SBA_CHOLMOD
77  chA = NULL;
78  chInited = false;
79  Common.print=0;
80 #endif
81  asize = 0;
82  csize = 0;
83  nnz = 0;
84  Bprev.resize(0);
85  }
86 
88  {
89  if (A) cs_spfree(A); // free any previous structure
90  if (AF) cs_spfree(AF); // free any previous structure
91  }
92 
93 
94  // 3x3 sparse jacobian blocks
95  void CSparse2d::setupBlockStructure(int n, bool eraseit)
96  {
97  // set up initial structure
98  diag.resize(n);
99  cols.resize(n);
100  if (eraseit)
101  {
102  for (int i=0; i<(int)cols.size(); i++)
103  {
104  map<int,Matrix<double,3,3>, less<int>,
105  aligned_allocator< Matrix <double,3,3> > > &col = cols[i];
106  col.clear();
107  }
108  }
109  asize = n;
110  csize = 3*n;
111 
112  if (eraseit)
113  {
114  // zero out entries
115  B.setZero(csize);
116  for (int i=0; i<(int)diag.size(); i++)
117  {
118  diag[i].setZero();
119  }
120  for (int i=0; i<(int)cols.size(); i++)
121  {
122  map<int,Matrix<double,3,3>, less<int>,
123  aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
124  if (col.size() > 0)
125  {
126  map<int,Matrix<double,3,3>, less<int>,
127  aligned_allocator<Matrix<double,3,3> > >::iterator it;
128  for (it = col.begin(); it != col.end(); it++)
129  {
130  it->second.setZero();
131  }
132  }
133  }
134  }else // here we just resize B, saving the old parts
135  {
136  B.setZero(csize);
137  if (Bprev.size() > 0)
138  {
139  B.head(Bprev.size()) = Bprev;
140  }
141  }
142  }
143 
144 
145  // add an off-diagonal block
146  void CSparse2d::addOffdiagBlock(Matrix<double,3,3> &m, int ii, int jj)
147  {
148  // get column
149  map<int,Matrix<double,3,3>, less<int>,
150  aligned_allocator<Matrix<double,3,3> > > &col = cols[jj];
151  // find current matrix
152  map<int,Matrix<double,3,3>, less<int>,
153  aligned_allocator<Matrix<double,3,3> > >::iterator it;
154  it = col.find(ii);
155  if (it == col.end()) // didn't find it
156  col.insert(pair<int,Matrix<double,3,3> >(ii,m));
157  else // found it, add
158  it->second += m;
159  }
160 
161 
162  void CSparse2d::incDiagBlocks(double lam)
163  {
164  for (int i=0; i<(int)diag.size(); i++)
165  diag[i].diagonal() *= lam;
166  }
167 
168 
169  // set up CSparse2d structure; <init> true if first time
170  // <diaginc> is the diagonal multiplier for LM
171 
172  // this version only sets upper triangular matrix,
173  // should set whole thing
174 
175  void CSparse2d::setupCSstructure(double diaginc, bool init)
176  {
177 #ifdef SBA_CHOLMOD
178  if (useCholmod) {
179  cholmod_start(&Common); // this is finished in doChol()
180  Common.print = 0;
181  }
182 #endif
183 
184  // reserve space and set things up
185  if (init || useCholmod)
186  {
187  if (A) cs_spfree(A); // free any previous structure
188 
189  // count entries for cs allocation
190  nnz = 6*asize; // diagonal entries, just upper triangle
191  for (int i=0; i<(int)cols.size(); i++)
192  {
193  map<int,Matrix<double,3,3>, less<int>,
194  aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
195  nnz += 9 * col.size(); // 3x3 matrix
196  }
197 #ifdef SBA_CHOLMOD
198  if (useCholmod)
199  {
200  // cholmod_start(&Common); // this is finished in doChol()
201  // if (chA)
202  // cholmod_free_sparse(&chA, &Common);
203  chA = cholmod_allocate_sparse(csize,csize,nnz,true,true,1,CHOLMOD_REAL,&Common);
204  }
205  else
206 #endif
207  {
208  A = cs_spalloc(csize,csize,nnz,1,0); // allocate sparse matrix
209  }
210 
211  // now figure out the column pointers
212  int colp = 0; // index of where the column starts in Ai
213  int *Ap, *Ai;
214 #ifdef SBA_CHOLMOD
215  if (useCholmod)
216  {
217  Ap = (int *)chA->p; // column pointer
218  Ai = (int *)chA->i; // row indices
219  }
220  else
221 #endif
222  {
223  Ap = A->p; // column pointer
224  Ai = A->i; // row indices
225  }
226 
227  for (int i=0; i<(int)cols.size(); i++)
228  {
229  // column i entries
230  map<int,Matrix<double,3,3>, less<int>,
231  aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
232 
233  // do this for 3 columns
234  for (int k=0; k<3; k++)
235  {
236  *Ap++ = colp;
237  int row;
238 
239  // iterate over the map
240  if (col.size() > 0)
241  {
242  // map iterator
243  map<int,Matrix<double,3,3>, less<int>,
244  aligned_allocator<Matrix<double,3,3> > >::iterator it;
245 
246  // iterate over block column entries
247  for (it = col.begin(); it != col.end(); it++)
248  {
249  row = 3*it->first; // which row we're at
250  for (int j=0; j<3; j++)
251  Ai[colp++] = row++;
252  }
253  }
254 
255  // add in diagonal entries
256  row = 3*i;
257  for (int kk=0; kk<k+1; kk++)
258  Ai[colp++] = row++;
259  }
260  }
261  *Ap = nnz; // last entry
262  }
263 
264  // now put the entries in place
265  int colp = 0; // index of where the column starts in Ai
266  double *Ax;
267 #ifdef SBA_CHOLMOD
268  if (useCholmod)
269  Ax = (double *)chA->x; // values
270  else
271 #endif
272  Ax = A->x; // values
273 
274  for (int i=0; i<(int)cols.size(); i++)
275  {
276  // column i entries
277  map<int,Matrix<double,3,3>, less<int>,
278  aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
279 
280  // do this for 3 columns
281  for (int k=0; k<3; k++)
282  {
283  // iterate over the map
284  if (col.size() > 0)
285  {
286  // map iterator
287  map<int,Matrix<double,3,3>, less<int>,
288  aligned_allocator<Matrix<double,3,3> > >::iterator it;
289 
290  // iterate over block column entries
291  for (it = col.begin(); it != col.end(); it++)
292  {
293  Matrix<double,3,3> &m = it->second;
294  for (int j=0; j<3; j++)
295  Ax[colp++] = m(j,k);
296  }
297  }
298 
299  // add in diagonal entries
300  Matrix<double,3,3> &m = diag[i]; // diagonal block
301  for (int kk=0; kk<k+1; kk++)
302  Ax[colp++] = m(kk,k);
303  Ax[colp-1] *= diaginc; // increment diagonal for LM
304  }
305  }
306 
307  // make symmetric from upper diagonal
308  // this could be more efficient if AT were eliminated and AF were fixed
309  // oops - chol_sol only needs upper diag
310  }
311 
312 
313  // uncompress the compressed A into a dense Eigen matrix
314  void CSparse2d::uncompress(MatrixXd &m)
315  {
316  if (!A) return;
317  m.setZero(csize,csize);
318 
319  int *Ap = A->p; // column pointer
320  int *Ai = A->i; // row indices
321  double *Ax = A->x; // values;
322 
323  for (int i=0; i<csize; i++)
324  {
325  int rbeg = Ap[i];
326  int rend = Ap[i+1];
327  if (rend > rbeg)
328  for (int j=rbeg; j<rend; j++)
329  m(Ai[j],i) = Ax[j];
330  }
331  }
332 
333  // solve in place, returns RHS B
335  {
336 #ifdef SBA_CHOLMOD
337  if (useCholmod)
338  {
339  cholmod_dense *x, b, *R, *R2;
340  cholmod_factor *L ;
341  double *Xx, *Rx, *bb;
342  double one [2], minusone [2];
343  one [0] = 1 ;
344  one [1] = 0 ;
345  minusone [0] = -1 ;
346  minusone [1] = 0 ;
347 
348  // cholmod_start (&Common) ; // start it up ???
349  cholmod_print_sparse (chA, (char *)"A", &Common) ; // print simple stats
350  b.nrow = csize;
351  b.ncol = 1;
352  b.d = csize; // leading dimension (???)
353  b.nzmax = csize;
354  b.xtype = CHOLMOD_REAL;
355  b.dtype = CHOLMOD_DOUBLE;
356  b.x = B.data();
357  //cout << "CHOLMOD analyze..." << flush;
358  L = cholmod_analyze (chA, &Common) ; // analyze
359  //cout << "factorize..." << flush;
360  cholmod_factorize (chA, L, &Common) ; // factorize
361  //cout << "solve..." << flush;
362  x = cholmod_solve (CHOLMOD_A, L, &b, &Common) ; // solve Ax=b
363  // cholmod_print_factor (L, (char *)"L", &Common) ;
364 
365  //cout << "refine" << endl;
366  // one step of iterative refinement, cheap
367  /* Ax=b was factorized and solved, R = B-A*X */
368  R = cholmod_copy_dense (&b, &Common) ;
369  cholmod_sdmult(chA, 0, minusone, one, x, R, &Common) ;
370  /* R2 = A\(B-A*X) */
371  R2 = cholmod_solve (CHOLMOD_A, L, R, &Common) ;
372  /* compute X = X + A\(B-A*X) */
373  Xx = (double *)x->x ;
374  Rx = (double *)R2->x ;
375  for (int i=0 ; i<csize ; i++)
376  {
377  Xx[i] = Xx[i] + Rx[i] ;
378  }
379  cholmod_free_dense (&R2, &Common) ;
380  cholmod_free_dense (&R, &Common) ;
381 
382  bb = B.data();
383  for (int i=0; i<csize; i++) // transfer answer
384  *bb++ = *Xx++;
385  cholmod_free_factor (&L, &Common) ; // free matrices
386  cholmod_free_dense (&x, &Common) ;
387  cholmod_free_sparse(&chA, &Common);
388  cholmod_finish (&Common) ; // finish it ???
389 
390  return true;
391  }
392  else
393 #endif
394  {
395 
396  // using order 0 here (natural order);
397  // may be better to use "1" for large problems (AMD)
398  int order = 0;
399  if (csize > 100) order = 1;
400  bool ok = (bool)cs_cholsol(order,A,B.data()); // do the CSparse2d thang
401  return ok;
402  }
403  }
404 
405 
406  //
407  // block jacobian PCG
408  // max iterations <iter>, ending toleranace <tol>
409  //
410 /*
411  int
412  CSparse2d::doBPCG(int iters, double tol, int sba_iter)
413  {
414  int n = B.rows();
415  VectorXd x;
416  x.setZero(n);
417  bool abstol = false;
418  if (sba_iter > 0) abstol = true;
419  int ret;
420  ret = bpcg.doBPCG2(iters, tol, diag, cols, x, B, abstol);
421  B = x; // transfer result data
422  return ret;
423  }
424 */
425 
426 #ifdef SBA_DSIF
427  // solve in place, returns RHS B
428  // PCG algorithm from SparseLib++, IML++
429  int CSparse2d::doPCG(int iters)
430  {
431  // first convert sparse matrix to SparseLib++ format
432  // do we need just upper triangular here???
433 
434  // add in lower triangular part
435  cs *AT;
436  AT = cs_transpose(A,1);
437  cs_fkeep (AT, &dropdiag, NULL); // drop diagonal entries from AT
438  if (AF) cs_spfree(AF); // free any previous structure
439  AF = cs_add (A, AT, 1, 1); // AF = A+AT
440  cs_spfree (AT);
441 
442  // convert to SparseLib objects
443  CompCol_Mat_double Ap(csize, csize, AF->nzmax, AF->x, AF->i, AF->p);
444  VECTOR_double b(B.data(),csize); // Create rhs
445  VECTOR_double x(csize, 0.0); // solution vectors
446 
447  // perform PCG
448  int res;
449  double tol = 1e-6;
450  ICPreconditioner_double D(Ap); // Create diagonal preconditioner
451  res = CG(Ap,x,b,D,iters,tol); // Solve system
452 
453  for (int i=0; i<csize; i++)
454  B[i] = x[i];
455 
456  // cout << "CG flag = " << res << endl;
457  // cout << "iterations performed: " << iters << endl;
458  // cout << "tolerance achieved : " << tol << endl;
459  // cout << x << endl;
460 
461  return res;
462  }
463 #endif
void uncompress(MatrixXd &m)
Definition: csparse.cpp:314
void incDiagBlocks(double lam)
Definition: csparse.cpp:162
void setupCSstructure(double diaginc, bool init=false)
Definition: csparse.cpp:175
TFSIMD_FORCE_INLINE const tfScalar & x() const
~CSparse2d()
Definition: csparse.cpp:87
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CSparse2d()
Definition: csparse.cpp:72
int doPCG(int iters)
void setupBlockStructure(int n, bool eraseit=true)
Definition: csparse.cpp:95
bool doChol()
Definition: csparse.cpp:334
void addOffdiagBlock(Matrix< double, 3, 3 > &m, int ii, int jj)
Definition: csparse.cpp:146


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:24