csparse_helper.h
Go to the documentation of this file.
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Wed Dec 26 2012 15:36:47