$search
00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds 00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss 00003 // 00004 // HOG-Man is free software: you can redistribute it and/or modify 00005 // it under the terms of the GNU Lesser General Public License as published 00006 // by the Free Software Foundation, either version 3 of the License, or 00007 // (at your option) any later version. 00008 // 00009 // HOG-Man is distributed in the hope that it will be useful, 00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 // GNU Lesser General Public License for more details. 00013 // 00014 // You should have received a copy of the GNU Lesser General Public License 00015 // along with this program. If not, see <http://www.gnu.org/licenses/>. 00016 00017 #ifndef CSPARSE_HELPER_H 00018 #define CSPARSE_HELPER_H 00019 00020 #include <algorithm> 00021 #include <vector> 00022 extern "C" { 00023 #include "hogman_minimal/csparse/cs.h" 00024 }; 00025 00026 namespace AISNavigation { 00027 00028 struct SparseMatrixEntry{ 00029 SparseMatrixEntry(int r=-1, int c=-1, double x=0.) : 00030 _r(r), _c(c), _x(x) 00031 { 00032 } 00033 inline void set(int r, int c, double x) 00034 { 00035 _r = r; 00036 _c = c; 00037 _x = x; 00038 } 00039 bool operator < (const SparseMatrixEntry& se) const { 00040 return _r<se._r || (_r==se._r && _c<se._c); 00041 } 00042 int _r,_c; 00043 double _x; 00044 }; 00045 00046 struct SparseMatrixEntryPtrCmp 00047 { 00048 bool operator()(const SparseMatrixEntry* e1, const SparseMatrixEntry* e2) const 00049 { 00050 return e1->_r < e2->_r || (e1->_r == e2->_r && e1->_c < e2->_c); 00051 } 00052 }; 00053 00054 inline cs_sparse* SparseMatrixEntryVector2CSparse(SparseMatrixEntry* entries, int r, int c, int nz) 00055 { 00056 std::sort(entries, entries+nz); 00057 struct cs_sparse* _csA = cs_spalloc(r,c, nz, 1, 1); 00058 if (! _csA) 00059 return 0; 00060 int *cIdx=_csA->p; // column pointer 00061 int *rIdx=_csA->i; // row indices 00062 _csA->nz=0; 00063 double* values=_csA->x; 00064 for(int i=0; i<nz; i++){ 00065 if (entries->_r==-1 || entries->_c==-1){ 00066 entries++; 00067 continue; 00068 } 00069 *values=entries->_x; 00070 *rIdx=entries->_r; 00071 *cIdx=entries->_c; 00072 values++; 00073 rIdx++; 00074 cIdx++; 00075 _csA->nz++; 00076 entries++; 00077 } 00078 return _csA; 00079 } 00080 00085 inline cs_sparse* SparseMatrixEntryPtrVector2CSparse(SparseMatrixEntry** entries, int r, int c, int nz) 00086 { 00087 struct cs_sparse* _csA = cs_spalloc(r,c, nz, 1, 1); 00088 if (! _csA) 00089 return 0; 00090 int *cIdx=_csA->p; // column pointer 00091 int *rIdx=_csA->i; // row indices 00092 double* values=_csA->x; 00093 _csA->nz = 0; 00094 for(int i = 0; i < nz; ++i) { 00095 SparseMatrixEntry* entry = *entries; 00096 if (entry->_r==-1 || entry->_c==-1){ 00097 entries++; 00098 continue; 00099 } 00100 *values = entry->_x; 00101 *rIdx = entry->_r; 00102 *cIdx = entry->_c; 00103 ++values; 00104 ++rIdx; 00105 ++cIdx; 00106 ++_csA->nz; 00107 ++entries; 00108 } 00109 return _csA; 00110 } 00111 00112 // our extensions to csparse 00113 csn* cs_chol_workspace (const cs *A, const css *S, int* cin, double* xin); 00114 int cs_cholsolsymb(const cs *A, double *b, const css* S, double* workspace, int* work); 00115 int cs_cholsolinvblocksymb(const cs *A, double **block, int r1, int c1, int r2,int c2, double* y, const css* S, double* x, double* b, double* temp, int* work); 00116 00117 } // end namespace 00118 00119 #endif