g2o
csparse_helper.cpp
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
3 //
4 // g2o is free software: you can redistribute it and/or modify
5 // it under the terms of the GNU Lesser General Public License as published
6 // by the Free Software Foundation, either version 3 of the License, or
7 // (at your option) any later version.
8 //
9 // g2o is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU Lesser General Public License for more details.
13 //
14 // You should have received a copy of the GNU Lesser General Public License
15 // along with this program. If not, see <http://www.gnu.org/licenses/>.
16 
17 #include "csparse_helper.h"
18 
19 #include "g2o/stuff/macros.h"
20 
21 #include <string>
22 #include <cassert>
23 #include <fstream>
24 #include <cstring>
25 #include <iomanip>
26 #include <iostream>
27 #include <algorithm>
28 #include <vector>
29 
30 using namespace std;
31 
32 namespace g2o {
33 namespace csparse_extension {
34 
36  SparseMatrixEntry(int r=-1, int c=-1, double x=0.) :
37  _r(r), _c(c), _x(x)
38  {
39  }
40  int _r,_c;
41  double _x;
42  };
43 
45  {
46  bool operator()(const SparseMatrixEntry& e1, const SparseMatrixEntry& e2) const
47  {
48  return e1._c < e2._c || (e1._c == e2._c && e1._r < e2._r);
49  }
50  };
51 
56  int cs_cholsolsymb(const cs *A, double *b, const css* S, double* x, int* work)
57  {
58  csn *N ;
59  int n, ok ;
60  if (!CS_CSC (A) || !b || ! S || !x) {
61  fprintf(stderr, "%s: No valid input!\n", __PRETTY_FUNCTION__);
62  assert(0); // get a backtrace in debug mode
63  return (0) ; /* check inputs */
64  }
65  n = A->n ;
66  N = cs_chol_workspace (A, S, work, x) ; /* numeric Cholesky factorization */
67  if (!N) {
68  fprintf(stderr, "%s: cholesky failed!\n", __PRETTY_FUNCTION__);
69  /*assert(0);*/
70  }
71  ok = (N != NULL) ;
72  if (ok)
73  {
74  cs_ipvec (S->pinv, b, x, n) ; /* x = P*b */
75  cs_lsolve (N->L, x) ; /* x = L\x */
76  cs_ltsolve (N->L, x) ; /* x = L'\x */
77  cs_pvec (S->pinv, x, b, n) ; /* b = P'*x */
78  }
79  cs_nfree (N) ;
80  return (ok) ;
81  }
82 
87  /* L = chol (A, [pinv parent cp]), pinv is optional */
88  csn* cs_chol_workspace (const cs *A, const css *S, int* cin, double* xin)
89  {
90  double d, lki, *Lx, *x, *Cx ;
91  int top, i, p, k, n, *Li, *Lp, *cp, *pinv, *s, *c, *parent, *Cp, *Ci ;
92  cs *L, *C, *E ;
93  csn *N ;
94  if (!CS_CSC (A) || !S || !S->cp || !S->parent) return (NULL) ;
95  n = A->n ;
96  N = (csn*) cs_calloc (1, sizeof (csn)) ; /* allocate result */
97  c = cin ; /* get int workspace */
98  x = xin ; /* get double workspace */
99  cp = S->cp ; pinv = S->pinv ; parent = S->parent ;
100  C = pinv ? cs_symperm (A, pinv, 1) : ((cs *) A) ;
101  E = pinv ? C : NULL ; /* E is alias for A, or a copy E=A(p,p) */
102  if (!N || !c || !x || !C) return (cs_ndone (N, E, NULL, NULL, 0)) ;
103  s = c + n ;
104  Cp = C->p ; Ci = C->i ; Cx = C->x ;
105  N->L = L = cs_spalloc (n, n, cp [n], 1, 0) ; /* allocate result */
106  if (!L) return (cs_ndone (N, E, NULL, NULL, 0)) ;
107  Lp = L->p ; Li = L->i ; Lx = L->x ;
108  for (k = 0 ; k < n ; k++) Lp [k] = c [k] = cp [k] ;
109  for (k = 0 ; k < n ; k++) /* compute L(k,:) for L*L' = C */
110  {
111  /* --- Nonzero pattern of L(k,:) ------------------------------------ */
112  top = cs_ereach (C, k, parent, s, c) ; /* find pattern of L(k,:) */
113  x [k] = 0 ; /* x (0:k) is now zero */
114  for (p = Cp [k] ; p < Cp [k+1] ; p++) /* x = full(triu(C(:,k))) */
115  {
116  if (Ci [p] <= k) x [Ci [p]] = Cx [p] ;
117  }
118  d = x [k] ; /* d = C(k,k) */
119  x [k] = 0 ; /* clear x for k+1st iteration */
120  /* --- Triangular solve --------------------------------------------- */
121  for ( ; top < n ; top++) /* solve L(0:k-1,0:k-1) * x = C(:,k) */
122  {
123  i = s [top] ; /* s [top..n-1] is pattern of L(k,:) */
124  lki = x [i] / Lx [Lp [i]] ; /* L(k,i) = x (i) / L(i,i) */
125  x [i] = 0 ; /* clear x for k+1st iteration */
126  for (p = Lp [i] + 1 ; p < c [i] ; p++)
127  {
128  x [Li [p]] -= Lx [p] * lki ;
129  }
130  d -= lki * lki ; /* d = d - L(k,i)*L(k,i) */
131  p = c [i]++ ;
132  Li [p] = k ; /* store L(k,i) in column i */
133  Lx [p] = lki ;
134  }
135  /* --- Compute L(k,k) ----------------------------------------------- */
136  if (d <= 0) return (cs_ndone (N, E, NULL, NULL, 0)) ; /* not pos def */
137  p = c [k]++ ;
138  Li [p] = k ; /* store L(k,k) = sqrt (d) in column k */
139  Lx [p] = sqrt (d) ;
140  }
141  Lp [n] = cp [n] ; /* finalize L */
142  return (cs_ndone (N, E, NULL, NULL, 1)) ; /* success: free E,s,x; return N */
143  }
144 
145  bool writeCs2Octave(const char* filename, const cs* A, bool upperTriangular)
146  {
147  int cols = A->n;
148  int rows = A->m;
149 
150  string name = filename;
151  std::string::size_type lastDot = name.find_last_of('.');
152  if (lastDot != std::string::npos)
153  name = name.substr(0, lastDot);
154 
155  vector<SparseMatrixEntry> entries;
156  if (A->nz == -1) { // CCS matrix
157  const int* Ap = A->p;
158  const int* Ai = A->i;
159  const double* Ax = A->x;
160  for (int i=0; i < cols; i++) {
161  const int& rbeg = Ap[i];
162  const int& rend = Ap[i+1];
163  for (int j = rbeg; j < rend; j++) {
164  entries.push_back(SparseMatrixEntry(Ai[j], i, Ax[j]));
165  if (upperTriangular && Ai[j] != i)
166  entries.push_back(SparseMatrixEntry(i, Ai[j], Ax[j]));
167  }
168  }
169  } else { // Triplet matrix
170  entries.reserve(A->nz);
171  int *Aj = A->p; // column indeces
172  int *Ai = A->i; // row indices
173  double *Ax = A->x; // values;
174  for (int i = 0; i < A->nz; ++i) {
175  entries.push_back(SparseMatrixEntry(Ai[i], Aj[i], Ax[i]));
176  if (upperTriangular && Ai[i] != Aj[i])
177  entries.push_back(SparseMatrixEntry(Aj[i], Ai[i], Ax[i]));
178  }
179  }
180  sort(entries.begin(), entries.end(), SparseMatrixEntryColSort());
181 
182  std::ofstream fout(filename);
183  fout << "# name: " << name << std::endl;
184  fout << "# type: sparse matrix" << std::endl;
185  fout << "# nnz: " << entries.size() << std::endl;
186  fout << "# rows: " << rows << std::endl;
187  fout << "# columns: " << cols << std::endl;
188  //fout << fixed;
189  fout << setprecision(9) << endl;
190 
191  for (vector<SparseMatrixEntry>::const_iterator it = entries.begin(); it != entries.end(); ++it) {
192  const SparseMatrixEntry& entry = *it;
193  fout << entry._r+1 << " " << entry._c+1 << " " << entry._x << std::endl;
194  }
195  return fout.good();
196  }
197 
198 } // end namespace
199 } // end namespace
#define __PRETTY_FUNCTION__
Definition: macros.h:89
SparseMatrixEntry(int r=-1, int c=-1, double x=0.)
int cs_cholsolsymb(const cs *A, double *b, const css *S, double *x, int *work)
bool operator()(const SparseMatrixEntry &e1, const SparseMatrixEntry &e2) const
csn * cs_chol_workspace(const cs *A, const css *S, int *cin, double *xin)
bool writeCs2Octave(const char *filename, const cs *A, bool upperTriangular)