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 
39 #include <stdio.h>
41 
42 using namespace Eigen;
43 
44 #include <iostream>
45 #include <iomanip>
46 #include <fstream>
47 #include <algorithm>
48 
49 using namespace std;
50 
51 
52 //
53 // Efficient interface to the CSparse sparse solver
54 //
55 // Basic idea is to store 6x6 blocks in a sparse structure,
56 // and when we're done forming the A matrix, transfer to
57 // a CSparse compressed structure. The latter only has
58 // to be initialized once at the start of the nonlinear iterations.
59 //
60 
61 //
62 // CSparse holds its sparse NxN arrays in compressed column format:
63 //
64 // col_ptr has N+1 entries, one for each column, and a final NNZ entry.
65 // each entry is the index in row_ptr of where the column starts.
66 // row_ind has NNZ entries, each one a row index for an element.
67 // vals has NNZ entries corresponding to the row_ptr entries
68 //
69 
70 namespace sba
71 {
72 
73  CSparse::CSparse()
74  {
75  A = NULL;
76 #ifdef SBA_CHOLMOD
77  chA = NULL;
78  chInited = false;
79 #endif
80  useCholmod = false;
81  asize = 0;
82  csize = 0;
83  nnz = 0;
84  }
85 
86  CSparse::~CSparse()
87  {
88  if (A) cs_spfree(A); // free any previous structure
89  }
90 
91 
92  // 6x6 sparse jacobian blocks
93  void CSparse::setupBlockStructure(int n)
94  {
95  if (n > 0) // set up initial structure
96  {
97  diag.clear();
98  diag.resize(n);
99  cols.clear();
100  cols.resize(n);
101  asize = n;
102  csize = 6*n;
103  }
104 
105  // zero out entries
106  B.setZero(csize);
107  for (int i=0; i<(int)diag.size(); i++)
108  diag[i].setZero();
109  for (int i=0; i<(int)cols.size(); i++)
110  {
111  map<int,Matrix<double,6,6>, less<int>,
112  aligned_allocator<Matrix<double,6,6> > > &col = cols[i];
113  if (col.size() > 0)
114  {
115  map<int,Matrix<double,6,6>, less<int>,
116  aligned_allocator<Matrix<double,6,6> > >::iterator it;
117  for (it = col.begin(); it != col.end(); it++)
118  it->second.setZero();
119  }
120  }
121  }
122 
123  void CSparse::incDiagBlocks(double lam)
124  {
125  for (int i=0; i<(int)diag.size(); i++)
126  diag[i].diagonal() *= lam;
127  }
128 
129  // add an off-diagonal block
130  void CSparse::addOffdiagBlock(Matrix<double,6,6> &m, int ii, int jj)
131  {
132  // get column
133  map<int,Matrix<double,6,6>, less<int>,
134  aligned_allocator<Matrix<double,6,6> > > &col = cols[jj];
135  // find current matrix
136  map<int,Matrix<double,6,6>, less<int>,
137  aligned_allocator<Matrix<double,6,6> > >::iterator it;
138  it = col.find(ii);
139  if (it == col.end()) // didn't find it
140  col.insert(pair<int,Matrix<double,6,6> >(ii,m));
141  else // found it, add
142  it->second += m;
143  }
144 
145 
146  // set up CSparse structure; <init> true if first time
147  // <diaginc> is the diagonal multiplier for LM
148 
149  // this version sets upper triangular matrix,
150  void CSparse::setupCSstructure(double diaginc, bool init)
151  {
152 #ifdef SBA_CHOLMOD
153  if (useCholmod) {
154  cholmod_start(&Common); // this is finished in doChol()
155  Common.print = 0;
156  }
157 #endif
158 
159  // reserve space and set things up
160  if (init || useCholmod)
161  {
162  // count entries for cs allocation
163  nnz = 21*asize; // diagonal entries, just upper triangle
164  for (int i=0; i<(int)cols.size(); i++)
165  {
166  map<int,Matrix<double,6,6>, less<int>,
167  aligned_allocator<Matrix<double,6,6> > > &col = cols[i];
168  nnz += 36 * col.size(); // 6x6 matrix
169  }
170 
171 #ifdef SBA_CHOLMOD
172  if (useCholmod)
173  {
174  // cholmod_start(&Common); // this is finished in doChol()
175  // if (chA)
176  // cholmod_free_sparse(&chA, &Common);
177  chA = cholmod_allocate_sparse(csize,csize,nnz,true,true,1,CHOLMOD_REAL,&Common);
178  }
179  else
180 #endif
181  {
182  if (A) cs_spfree(A); // free any previous structure
183  A = cs_spalloc(csize,csize,nnz,1,0); // allocate sparse matrix
184  }
185 
186  // now figure out the column pointers
187  int colp = 0; // index of where the column starts in Ai
188  int *Ap, *Ai;
189 #ifdef SBA_CHOLMOD
190  if (useCholmod)
191  {
192  Ap = (int *)chA->p; // column pointer
193  Ai = (int *)chA->i; // row indices
194  }
195  else
196 #endif
197  {
198  Ap = A->p; // column pointer
199  Ai = A->i; // row indices
200  }
201  for (int i=0; i<(int)cols.size(); i++)
202  {
203  // column i entries
204  map<int,Matrix<double,6,6>, less<int>,
205  aligned_allocator<Matrix<double,6,6> > > &col = cols[i];
206 
207  // do this for 6 columns
208  for (int k=0; k<6; k++)
209  {
210  *Ap++ = colp;
211  int row;
212 
213  // iterate over the map
214  if (col.size() > 0)
215  {
216  // map iterator
217  map<int,Matrix<double,6,6>, less<int>,
218  aligned_allocator<Matrix<double,6,6> > >::iterator it;
219 
220  // iterate over block column entries
221  for (it = col.begin(); it != col.end(); it++)
222  {
223  row = 6*it->first; // which row we're at
224  for (int j=0; j<6; j++)
225  Ai[colp++] = row++;
226  }
227  }
228 
229  // add in diagonal entries
230  row = 6*i;
231  for (int kk=0; kk<k+1; kk++)
232  Ai[colp++] = row++;
233  }
234  }
235  *Ap = nnz; // last entry
236  }
237 
238 
239 
240  // now put the entries in place
241  int colp = 0; // index of where the column starts in Ai
242  double *Ax;
243 #ifdef SBA_CHOLMOD
244  if (useCholmod)
245  Ax = (double *)chA->x; // values
246  else
247 #endif
248  Ax = A->x; // values
249  for (int i=0; i<(int)cols.size(); i++)
250  {
251  // column i entries
252  map<int,Matrix<double,6,6>, less<int>,
253  aligned_allocator<Matrix<double,6,6> > > &col = cols[i];
254 
255  // do this for 6 columns
256  for (int k=0; k<6; k++)
257  {
258  // iterate over the map
259  if (col.size() > 0)
260  {
261  // map iterator
262  map<int,Matrix<double,6,6>, less<int>,
263  aligned_allocator<Matrix<double,6,6> > >::iterator it;
264 
265  // iterate over block column entries
266  for (it = col.begin(); it != col.end(); it++)
267  {
268  Matrix<double,6,6> &m = it->second;
269  for (int j=0; j<6; j++)
270  Ax[colp++] = m(j,k);
271  }
272  }
273 
274  // add in diagonal entries
275  Matrix<double,6,6> &m = diag[i]; // diagonal block
276  for (int kk=0; kk<k+1; kk++)
277  Ax[colp++] = m(kk,k);
278  Ax[colp-1] *= diaginc; // increment diagonal for LM
279  }
280  }
281 
282  }
283 
284 
285  // uncompress the compressed A into a dense Eigen matrix
286  void CSparse::uncompress(MatrixXd &m)
287  {
288  if (!A) return;
289  m.setZero(csize,csize);
290 
291  int *Ap = A->p; // column pointer
292  int *Ai = A->i; // row indices
293  double *Ax = A->x; // values;
294 
295  for (int i=0; i<csize; i++)
296  {
297  int rbeg = Ap[i];
298  int rend = Ap[i+1];
299  if (rend > rbeg)
300  for (int j=rbeg; j<rend; j++)
301  m(Ai[j],i) = Ax[j];
302  }
303  }
304 
305  // solve in place, returns RHS B
306  bool CSparse::doChol()
307  {
308 #ifdef SBA_CHOLMOD
309  if (useCholmod)
310  {
311  cholmod_dense *x, b, *R, *R2;
312  cholmod_factor *L ;
313  double *Xx, *Rx, *bb;
314  double one [2], minusone [2];
315  one [0] = 1 ;
316  one [1] = 0 ;
317  minusone [0] = -1 ;
318  minusone [1] = 0 ;
319 
320  // cholmod_start (&Common) ; // start it up ???
321  cholmod_print_sparse (chA, (char *)"A", &Common) ; // print simple stats
322  b.nrow = csize;
323  b.ncol = 1;
324  b.d = csize; // leading dimension (???)
325  b.nzmax = csize;
326  b.xtype = CHOLMOD_REAL;
327  b.dtype = CHOLMOD_DOUBLE;
328  b.x = B.data();
329  //cout << "CHOLMOD analyze..." << flush;
330  L = cholmod_analyze (chA, &Common) ; // analyze
331  //cout << "factorize..." << flush;
332  cholmod_factorize (chA, L, &Common) ; // factorize
333  //cout << "solve..." << flush;
334  x = cholmod_solve (CHOLMOD_A, L, &b, &Common) ; // solve Ax=b
335  // cholmod_print_factor (L, (char *)"L", &Common) ;
336 
337  //cout << "refine" << endl;
338  // one step of iterative refinement, cheap
339  /* Ax=b was factorized and solved, R = B-A*X */
340  R = cholmod_copy_dense (&b, &Common) ;
341  cholmod_sdmult(chA, 0, minusone, one, x, R, &Common) ;
342  /* R2 = A\(B-A*X) */
343  R2 = cholmod_solve (CHOLMOD_A, L, R, &Common) ;
344  /* compute X = X + A\(B-A*X) */
345  Xx = (double *)x->x ;
346  Rx = (double *)R2->x ;
347  for (int i=0 ; i<csize ; i++)
348  {
349  Xx[i] = Xx[i] + Rx[i] ;
350  }
351  cholmod_free_dense (&R2, &Common) ;
352  cholmod_free_dense (&R, &Common) ;
353 
354  bb = B.data();
355  for (int i=0; i<csize; i++) // transfer answer
356  *bb++ = *Xx++;
357  cholmod_free_factor (&L, &Common) ; // free matrices
358  cholmod_free_dense (&x, &Common) ;
359  cholmod_free_sparse(&chA, &Common);
360  cholmod_finish (&Common) ; // finish it ???
361 
362  return true;
363  }
364  else
365 #endif
366  {
367  // using order 0 here (natural order);
368  // may be better to use "1" for large problems
369  int order = 0;
370  if (csize > 400) order = 1;
371  bool ok = (bool)cs_cholsol(order,A,B.data()); // do the CSparse thang
372  return ok;
373  }
374  }
375 
376 
377  //
378  // block jacobian PCG
379  // max iterations <iter>, ending toleranace <tol>
380  //
381 
382  int
383  CSparse::doBPCG(int iters, double tol, int sba_iter)
384  {
385  int n = B.rows();
386  VectorXd x;
387  x.setZero(n);
388  bool abstol = false;
389  if (sba_iter > 0) abstol = true;
390  int ret;
391  ret = bpcg.doBPCG2(iters, tol, diag, cols, x, B, abstol);
392  B = x; // transfer result data
393  return ret;
394  }
395 
396 
397  //
398  // 2d version
399  //
400 
401  CSparse2d::CSparse2d()
402  {
403  A = NULL;
404  AF = NULL;
405 #ifdef SBA_CHOLMOD
406  chA = NULL;
407  chInited = false;
408  Common.print=0;
409 #endif
410  useCholmod = false;
411  asize = 0;
412  csize = 0;
413  nnz = 0;
414  Bprev.resize(0);
415  }
416 
417  CSparse2d::~CSparse2d()
418  {
419  if (A) cs_spfree(A); // free any previous structure
420  if (AF) cs_spfree(AF); // free any previous structure
421  }
422 
423 
424  // 3x3 sparse jacobian blocks
425  void CSparse2d::setupBlockStructure(int n, bool eraseit)
426  {
427  if (n > 0) // set up initial structure
428  {
429  diag.resize(n);
430  cols.resize(n);
431  if (eraseit)
432  for (int i=0; i<(int)cols.size(); i++)
433  {
434  map<int,Matrix<double,3,3>, less<int>,
435  aligned_allocator< Matrix <double,3,3> > > &col = cols[i];
436  col.clear();
437  }
438  asize = n;
439  csize = 3*n;
440  }
441 
442  if (eraseit)
443  {
444  // zero out entries
445  B.setZero(csize);
446  for (int i=0; i<(int)diag.size(); i++)
447  diag[i].setZero();
448  for (int i=0; i<(int)cols.size(); i++)
449  {
450  map<int,Matrix<double,3,3>, less<int>,
451  aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
452  if (col.size() > 0)
453  {
454  map<int,Matrix<double,3,3>, less<int>,
455  aligned_allocator<Matrix<double,3,3> > >::iterator it;
456  for (it = col.begin(); it != col.end(); it++)
457  it->second.setZero();
458  }
459  }
460  }
461 
462  else // here we just resize B, saving the old parts
463  {
464  B.setZero(csize);
465  if (Bprev.size() > 0)
466  B.head(Bprev.size()) = Bprev;
467  }
468  }
469 
470 
471  // add an off-diagonal block
472  void CSparse2d::addOffdiagBlock(Matrix<double,3,3> &m, int ii, int jj)
473  {
474  // get column
475  map<int,Matrix<double,3,3>, less<int>,
476  aligned_allocator<Matrix<double,3,3> > > &col = cols[jj];
477  // find current matrix
478  map<int,Matrix<double,3,3>, less<int>,
479  aligned_allocator<Matrix<double,3,3> > >::iterator it;
480  it = col.find(ii);
481  if (it == col.end()) // didn't find it
482  col.insert(pair<int,Matrix<double,3,3> >(ii,m));
483  else // found it, add
484  it->second += m;
485  }
486 
487 
488  void CSparse2d::incDiagBlocks(double lam)
489  {
490  for (int i=0; i<(int)diag.size(); i++)
491  diag[i].diagonal() *= lam;
492  }
493 
494 
495  // set up CSparse2d structure; <init> true if first time
496  // <diaginc> is the diagonal multiplier for LM
497 
498  // this version only sets upper triangular matrix,
499  // should set whole thing
500 
501  void CSparse2d::setupCSstructure(double diaginc, bool init)
502  {
503 #ifdef SBA_CHOLMOD
504  if (useCholmod) {
505  cholmod_start(&Common); // this is finished in doChol()
506  Common.print = 0;
507  }
508 #endif
509 
510  // reserve space and set things up
511  if (init || useCholmod)
512  {
513  if (A) cs_spfree(A); // free any previous structure
514 
515  // count entries for cs allocation
516  nnz = 6*asize; // diagonal entries, just upper triangle
517  for (int i=0; i<(int)cols.size(); i++)
518  {
519  map<int,Matrix<double,3,3>, less<int>,
520  aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
521  nnz += 9 * col.size(); // 3x3 matrix
522  }
523 #ifdef SBA_CHOLMOD
524  if (useCholmod)
525  {
526  // cholmod_start(&Common); // this is finished in doChol()
527  // if (chA)
528  // cholmod_free_sparse(&chA, &Common);
529  chA = cholmod_allocate_sparse(csize,csize,nnz,true,true,1,CHOLMOD_REAL,&Common);
530  }
531  else
532 #endif
533  {
534  A = cs_spalloc(csize,csize,nnz,1,0); // allocate sparse matrix
535  }
536 
537  // now figure out the column pointers
538  int colp = 0; // index of where the column starts in Ai
539  int *Ap, *Ai;
540 #ifdef SBA_CHOLMOD
541  if (useCholmod)
542  {
543  Ap = (int *)chA->p; // column pointer
544  Ai = (int *)chA->i; // row indices
545  }
546  else
547 #endif
548  {
549  Ap = A->p; // column pointer
550  Ai = A->i; // row indices
551  }
552 
553  for (int i=0; i<(int)cols.size(); i++)
554  {
555  // column i entries
556  map<int,Matrix<double,3,3>, less<int>,
557  aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
558 
559  // do this for 3 columns
560  for (int k=0; k<3; k++)
561  {
562  *Ap++ = colp;
563  int row;
564 
565  // iterate over the map
566  if (col.size() > 0)
567  {
568  // map iterator
569  map<int,Matrix<double,3,3>, less<int>,
570  aligned_allocator<Matrix<double,3,3> > >::iterator it;
571 
572  // iterate over block column entries
573  for (it = col.begin(); it != col.end(); it++)
574  {
575  row = 3*it->first; // which row we're at
576  for (int j=0; j<3; j++)
577  Ai[colp++] = row++;
578  }
579  }
580 
581  // add in diagonal entries
582  row = 3*i;
583  for (int kk=0; kk<k+1; kk++)
584  Ai[colp++] = row++;
585  }
586  }
587  *Ap = nnz; // last entry
588  }
589 
590  // now put the entries in place
591  int colp = 0; // index of where the column starts in Ai
592  double *Ax;
593 #ifdef SBA_CHOLMOD
594  if (useCholmod)
595  Ax = (double *)chA->x; // values
596  else
597 #endif
598  Ax = A->x; // values
599 
600  for (int i=0; i<(int)cols.size(); i++)
601  {
602  // column i entries
603  map<int,Matrix<double,3,3>, less<int>,
604  aligned_allocator<Matrix<double,3,3> > > &col = cols[i];
605 
606  // do this for 3 columns
607  for (int k=0; k<3; k++)
608  {
609  // iterate over the map
610  if (col.size() > 0)
611  {
612  // map iterator
613  map<int,Matrix<double,3,3>, less<int>,
614  aligned_allocator<Matrix<double,3,3> > >::iterator it;
615 
616  // iterate over block column entries
617  for (it = col.begin(); it != col.end(); it++)
618  {
619  Matrix<double,3,3> &m = it->second;
620  for (int j=0; j<3; j++)
621  Ax[colp++] = m(j,k);
622  }
623  }
624 
625  // add in diagonal entries
626  Matrix<double,3,3> &m = diag[i]; // diagonal block
627  for (int kk=0; kk<k+1; kk++)
628  Ax[colp++] = m(kk,k);
629  Ax[colp-1] *= diaginc; // increment diagonal for LM
630  }
631  }
632 
633  // make symmetric from upper diagonal
634  // this could be more efficient if AT were eliminated and AF were fixed
635  // oops - chol_sol only needs upper diag
636  }
637 
638 
639  // uncompress the compressed A into a dense Eigen matrix
640  void CSparse2d::uncompress(MatrixXd &m)
641  {
642  if (!A) return;
643  m.setZero(csize,csize);
644 
645  int *Ap = A->p; // column pointer
646  int *Ai = A->i; // row indices
647  double *Ax = A->x; // values;
648 
649  for (int i=0; i<csize; i++)
650  {
651  int rbeg = Ap[i];
652  int rend = Ap[i+1];
653  if (rend > rbeg)
654  for (int j=rbeg; j<rend; j++)
655  m(Ai[j],i) = Ax[j];
656  }
657  }
658 
659  // solve in place, returns RHS B
660  bool CSparse2d::doChol()
661  {
662 #ifdef SBA_CHOLMOD
663  if (useCholmod)
664  {
665  cholmod_dense *x, b, *R, *R2;
666  cholmod_factor *L ;
667  double *Xx, *Rx, *bb;
668  double one [2], minusone [2];
669  one [0] = 1 ;
670  one [1] = 0 ;
671  minusone [0] = -1 ;
672  minusone [1] = 0 ;
673 
674  // cholmod_start (&Common) ; // start it up ???
675  cholmod_print_sparse (chA, (char *)"A", &Common) ; // print simple stats
676  b.nrow = csize;
677  b.ncol = 1;
678  b.d = csize; // leading dimension (???)
679  b.nzmax = csize;
680  b.xtype = CHOLMOD_REAL;
681  b.dtype = CHOLMOD_DOUBLE;
682  b.x = B.data();
683  //cout << "CHOLMOD analyze..." << flush;
684  L = cholmod_analyze (chA, &Common) ; // analyze
685  //cout << "factorize..." << flush;
686  cholmod_factorize (chA, L, &Common) ; // factorize
687  //cout << "solve..." << flush;
688  x = cholmod_solve (CHOLMOD_A, L, &b, &Common) ; // solve Ax=b
689  // cholmod_print_factor (L, (char *)"L", &Common) ;
690 
691  //cout << "refine" << endl;
692  // one step of iterative refinement, cheap
693  /* Ax=b was factorized and solved, R = B-A*X */
694  R = cholmod_copy_dense (&b, &Common) ;
695  cholmod_sdmult(chA, 0, minusone, one, x, R, &Common) ;
696  /* R2 = A\(B-A*X) */
697  R2 = cholmod_solve (CHOLMOD_A, L, R, &Common) ;
698  /* compute X = X + A\(B-A*X) */
699  Xx = (double *)x->x ;
700  Rx = (double *)R2->x ;
701  for (int i=0 ; i<csize ; i++)
702  {
703  Xx[i] = Xx[i] + Rx[i] ;
704  }
705  cholmod_free_dense (&R2, &Common) ;
706  cholmod_free_dense (&R, &Common) ;
707 
708  bb = B.data();
709  for (int i=0; i<csize; i++) // transfer answer
710  *bb++ = *Xx++;
711  cholmod_free_factor (&L, &Common) ; // free matrices
712  cholmod_free_dense (&x, &Common) ;
713  cholmod_free_sparse(&chA, &Common);
714  cholmod_finish (&Common) ; // finish it ???
715 
716  return true;
717  }
718  else
719 #endif
720  {
721 
722  // using order 0 here (natural order);
723  // may be better to use "1" for large problems (AMD)
724  int order = 0;
725  if (csize > 100) order = 1;
726  bool ok = (bool)cs_cholsol(order,A,B.data()); // do the CSparse2d thang
727  return ok;
728  }
729  }
730 
731 
732  //
733  // block jacobian PCG
734  // max iterations <iter>, ending toleranace <tol>
735  //
736 
737  int
738  CSparse2d::doBPCG(int iters, double tol, int sba_iter)
739  {
740  int n = B.rows();
741  VectorXd x;
742  x.setZero(n);
743  bool abstol = false;
744  if (sba_iter > 0) abstol = true;
745  int ret;
746  ret = bpcg.doBPCG2(iters, tol, diag, cols, x, B, abstol);
747  B = x; // transfer result data
748  return ret;
749  }
750 
751 
752 #ifdef SBA_DSIF
753  // solve in place, returns RHS B
754  // PCG algorithm from SparseLib++, IML++
755  int CSparse2d::doPCG(int iters)
756  {
757  // first convert sparse matrix to SparseLib++ format
758  // do we need just upper triangular here???
759 
760  // add in lower triangular part
761  cs *AT;
762  AT = cs_transpose(A,1);
763  cs_fkeep (AT, &dropdiag, NULL); // drop diagonal entries from AT
764  if (AF) cs_spfree(AF); // free any previous structure
765  AF = cs_add (A, AT, 1, 1); // AF = A+AT
766  cs_spfree (AT);
767 
768  // convert to SparseLib objects
769  CompCol_Mat_double Ap(csize, csize, AF->nzmax, AF->x, AF->i, AF->p);
770  VECTOR_double b(B.data(),csize); // Create rhs
771  VECTOR_double x(csize, 0.0); // solution vectors
772 
773  // perform PCG
774  int res;
775  double tol = 1e-6;
776  ICPreconditioner_double D(Ap); // Create diagonal preconditioner
777  res = CG(Ap,x,b,D,iters,tol); // Solve system
778 
779  for (int i=0; i<csize; i++)
780  B[i] = x[i];
781 
782  // cout << "CG flag = " << res << endl;
783  // cout << "iterations performed: " << iters << endl;
784  // cout << "tolerance achieved : " << tol << endl;
785  // cout << x << endl;
786 
787  return res;
788  }
789 #endif
790 
791 
792 } // end namespace sba
Eigen
Definition: sba_file_io.h:18
csparse.h
ok
ROSCPP_DECL bool ok()
sba
Definition: bpcg.h:60
std
init
void init(const M_string &remappings)


sparse_bundle_adjustment
Author(s):
autogenerated on Wed Mar 2 2022 01:03:04