csparse_helper.cpp
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 #include "csparse_helper.h"
00018 
00019 #include <cassert>
00020 
00021 namespace AISNavigation {
00022 
00023 int cs_cholsolsymb(const cs *A, double *b, const css* S, double* x, int* work)
00024 {
00025   csn *N ;
00026   int n, ok ;
00027   if (!CS_CSC (A) || !b || ! S || !x) {
00028     fprintf(stderr, "%s: No valid input!\n", __PRETTY_FUNCTION__);
00029     assert(0); // get a backtrace in debug mode
00030     return (0) ;     /* check inputs */
00031   }
00032   n = A->n ;
00033   N = cs_chol_workspace (A, S, work, x) ;                    /* numeric Cholesky factorization */
00034   if (!N) {
00035     fprintf(stderr, "%s: cholesky failed!\n", __PRETTY_FUNCTION__);
00036     /*assert(0);*/
00037   }
00038   ok = (N != NULL) ;
00039   if (ok)
00040   {
00041     cs_ipvec (S->pinv, b, x, n) ;   /* x = P*b */
00042     cs_lsolve (N->L, x) ;           /* x = L\x */
00043     cs_ltsolve (N->L, x) ;          /* x = L'\x */
00044     cs_pvec (S->pinv, x, b, n) ;    /* b = P'*x */
00045   }
00046   cs_nfree (N) ;
00047   return (ok) ;
00048 }
00049 
00050 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)
00051 {
00052   csn *N ;
00053   int n, ok, i, j ;
00054   if (!CS_CSC (A) || !block || !y || !x || !b || !temp) return (0) ;     /* check inputs */
00055   n = A->n ;
00056   if(r2<=r1||c2<=c1) return (0);
00057   if(c1<0 || c2>n)
00058     return 0;
00059   if(r1<0 || r2>n)
00060     return 0;
00061 
00062   N = cs_chol_workspace (A, S, work, x) ;                    /* numeric Cholesky factorization */
00063   ok = (N != NULL) ;
00064   if (ok)
00065   {
00066     // solve the system
00067     cs_ipvec (S->pinv, y, x, n) ;   /* x = P*y */
00068     cs_lsolve (N->L, x) ;           /* x = L\x */
00069     cs_ltsolve (N->L, x) ;          /* x = L'\x */
00070     cs_pvec (S->pinv, x, y, n) ;    /* b = P'*x */
00071     // solve the inverse
00072 
00073     for (i=0; i<n; i++){
00074       b[i]=0.;
00075     }
00076     for (i=c1; i<c2; i++){
00077       b[i]=1.;
00078 
00079       cs_ipvec (S->pinv, b, x, n) ;   /* x = P*b */
00080       cs_lsolve (N->L, x) ;           /* x = L\x */
00081       cs_ltsolve (N->L, x) ;          /* x = L'\x */
00082       cs_pvec (S->pinv, x, temp, n) ;    /* b = P'*x */
00083       for (j=r1; j<r2; j++){
00084         block[j-r1][i-c1]=temp[j];
00085       }
00086       b[i]=0.;
00087     }
00088   } 
00089   cs_nfree (N) ;
00090   return (ok) ;
00091 }
00092 
00093 /* L = chol (A, [pinv parent cp]), pinv is optional */
00094 csn* cs_chol_workspace (const cs *A, const css *S, int* cin, double* xin)
00095 {
00096     double d, lki, *Lx, *x, *Cx ;
00097     int top, i, p, k, n, *Li, *Lp, *cp, *pinv, *s, *c, *parent, *Cp, *Ci ;
00098     cs *L, *C, *E ;
00099     csn *N ;
00100     if (!CS_CSC (A) || !S || !S->cp || !S->parent) return (NULL) ;
00101     n = A->n ;
00102     N = (csn*) cs_calloc (1, sizeof (csn)) ;       /* allocate result */
00103     c = cin ;     /* get int workspace */
00104     x = xin ;    /* get double workspace */
00105     cp = S->cp ; pinv = S->pinv ; parent = S->parent ;
00106     C = pinv ? cs_symperm (A, pinv, 1) : ((cs *) A) ;
00107     E = pinv ? C : NULL ;           /* E is alias for A, or a copy E=A(p,p) */
00108     if (!N || !c || !x || !C) return (cs_ndone (N, E, NULL, NULL, 0)) ;
00109     s = c + n ;
00110     Cp = C->p ; Ci = C->i ; Cx = C->x ;
00111     N->L = L = cs_spalloc (n, n, cp [n], 1, 0) ;    /* allocate result */
00112     if (!L) return (cs_ndone (N, E, NULL, NULL, 0)) ;
00113     Lp = L->p ; Li = L->i ; Lx = L->x ;
00114     for (k = 0 ; k < n ; k++) Lp [k] = c [k] = cp [k] ;
00115     for (k = 0 ; k < n ; k++)       /* compute L(k,:) for L*L' = C */
00116     {
00117         /* --- Nonzero pattern of L(k,:) ------------------------------------ */
00118         top = cs_ereach (C, k, parent, s, c) ;      /* find pattern of L(k,:) */
00119         x [k] = 0 ;                                 /* x (0:k) is now zero */
00120         for (p = Cp [k] ; p < Cp [k+1] ; p++)       /* x = full(triu(C(:,k))) */
00121         {
00122             if (Ci [p] <= k) x [Ci [p]] = Cx [p] ;
00123         }
00124         d = x [k] ;                     /* d = C(k,k) */
00125         x [k] = 0 ;                     /* clear x for k+1st iteration */
00126         /* --- Triangular solve --------------------------------------------- */
00127         for ( ; top < n ; top++)    /* solve L(0:k-1,0:k-1) * x = C(:,k) */
00128         {
00129             i = s [top] ;               /* s [top..n-1] is pattern of L(k,:) */
00130             lki = x [i] / Lx [Lp [i]] ; /* L(k,i) = x (i) / L(i,i) */
00131             x [i] = 0 ;                 /* clear x for k+1st iteration */
00132             for (p = Lp [i] + 1 ; p < c [i] ; p++)
00133             {
00134                 x [Li [p]] -= Lx [p] * lki ;
00135             }
00136             d -= lki * lki ;            /* d = d - L(k,i)*L(k,i) */
00137             p = c [i]++ ;
00138             Li [p] = k ;                /* store L(k,i) in column i */
00139             Lx [p] = lki ;
00140         }
00141         /* --- Compute L(k,k) ----------------------------------------------- */
00142         if (d <= 0) return (cs_ndone (N, E, NULL, NULL, 0)) ; /* not pos def */
00143         p = c [k]++ ;
00144         Li [p] = k ;                /* store L(k,k) = sqrt (d) in column k */
00145         Lx [p] = sqrt (d) ;
00146     }
00147     Lp [n] = cp [n] ;               /* finalize L */
00148     return (cs_ndone (N, E, NULL, NULL, 1)) ; /* success: free E,s,x; return N */
00149 }
00150 
00151 } // end namespace


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:58