sparse_matrix.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2009-2012, Willow Garage, Inc.
00006  *  Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho,
00007  *                      Johns Hopkins University
00008  *
00009  *  All rights reserved.
00010  *
00011  *  Redistribution and use in source and binary forms, with or without
00012  *  modification, are permitted provided that the following conditions
00013  *  are met:
00014  *
00015  *   * Redistributions of source code must retain the above copyright
00016  *     notice, this list of conditions and the following disclaimer.
00017  *   * Redistributions in binary form must reproduce the above
00018  *     copyright notice, this list of conditions and the following
00019  *     disclaimer in the documentation and/or other materials provided
00020  *     with the distribution.
00021  *   * Neither the name of Willow Garage, Inc. nor the names of its
00022  *     contributors may be used to endorse or promote products derived
00023  *     from this software without specific prior written permission.
00024  *
00025  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00026  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00027  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00028  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00029  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00030  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00031  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00032  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00033  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00034  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00035  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00036  *  POSSIBILITY OF SUCH DAMAGE.
00037  *
00038  * $Id: sparse_matrix.h 5531 2012-04-08 09:14:33Z aichim $
00039  *
00040  */
00041 
00042 #ifndef PCL_POISSON_SPARSE_MATRIX_H_
00043 #define PCL_POISSON_SPARSE_MATRIX_H_
00044 
00045 #include "vector.h"
00046 #include "allocator.h"
00047 
00048 namespace pcl 
00049 {
00050   namespace poisson 
00051   {
00052     template <class T>
00053     struct MatrixEntry
00054     {
00055       MatrixEntry () : N (-1), Value (0) {}
00056       MatrixEntry (int i) : N (i), Value (0) {}
00057       int N;
00058       T Value;
00059     };
00060 
00061     template <class T,int Dim>
00062     struct NMatrixEntry
00063     {
00064       NMatrixEntry () : N (-1), Value () { memset (Value, 0, sizeof (T) * Dim); }
00065       NMatrixEntry (int i) : N (i), Value () { memset (Value, 0, sizeof (T) * Dim); }
00066       int N;
00067       T Value[Dim];
00068     };
00069 
00070     template<class T> class SparseMatrix
00071     {
00072       private:
00073         static int UseAlloc;
00074       public:
00075         static Allocator<MatrixEntry<T> > AllocatorMatrixEntry;
00076         static int UseAllocator (void);
00077         static void SetAllocator (const int& blockSize);
00078 
00079         int rows;
00080         int* rowSizes;
00081         MatrixEntry<T>** m_ppElements;
00082 
00083         SparseMatrix ();
00084         SparseMatrix (int rows);
00085         void Resize (int rows);
00086         void SetRowSize (int row , int count);
00087         int Entries (void);
00088 
00089         SparseMatrix (const SparseMatrix& M);
00090         virtual ~SparseMatrix ();
00091 
00092         void SetZero ();
00093         void SetIdentity ();
00094 
00095         SparseMatrix<T>& operator = (const SparseMatrix<T>& M);
00096 
00097         SparseMatrix<T> operator * (const T& V) const;
00098         SparseMatrix<T>& operator *= (const T& V);
00099 
00100 
00101         SparseMatrix<T> operator * (const SparseMatrix<T>& M) const;
00102         SparseMatrix<T> Multiply (const SparseMatrix<T>& M) const;
00103         SparseMatrix<T> MultiplyTranspose (const SparseMatrix<T>& Mt) const;
00104 
00105         template<class T2>
00106         Vector<T2> operator * (const Vector<T2>& V) const;
00107         template<class T2>
00108         Vector<T2> Multiply (const Vector<T2>& V) const;
00109         template<class T2>
00110         void Multiply (const Vector<T2>& In, Vector<T2>& Out) const;
00111 
00112 
00113         SparseMatrix<T> Transpose() const;
00114 
00115         static int Solve (const SparseMatrix<T>& M,
00116                           const Vector<T>& b,
00117                           const int& iters,
00118                           Vector<T>& solution,
00119                           const T eps = 1e-8);
00120 
00121         template<class T2>
00122         static int SolveSymmetric (const SparseMatrix<T>& M,
00123                                    const Vector<T2>& b,
00124                                    const int& iters,
00125                                    Vector<T2>& solution,
00126                                    const T2 eps = 1e-8,
00127                                    const int& reset=1);
00128 
00129     };
00130 
00131     template<class T,int Dim> class SparseNMatrix
00132     {
00133       private:
00134         static int UseAlloc;
00135       public:
00136         static Allocator<NMatrixEntry<T,Dim> > AllocatorNMatrixEntry;
00137         static int UseAllocator (void);
00138         static void SetAllocator (const int& blockSize);
00139 
00140         int rows;
00141         int* rowSizes;
00142         NMatrixEntry<T,Dim>** m_ppElements;
00143 
00144         SparseNMatrix ();
00145         SparseNMatrix (int rows);
00146         void Resize (int rows);
00147         void SetRowSize (int row, int count);
00148         int Entries ();
00149 
00150         SparseNMatrix (const SparseNMatrix& M);
00151         ~SparseNMatrix ();
00152 
00153         SparseNMatrix& operator = (const SparseNMatrix& M);
00154 
00155         SparseNMatrix  operator *  (const T& V) const;
00156         SparseNMatrix& operator *= (const T& V);
00157 
00158         template<class T2>
00159         NVector<T2,Dim> operator * (const Vector<T2>& V) const;
00160         template<class T2>
00161         Vector<T2> operator * (const NVector<T2,Dim>& V) const;
00162     };
00163 
00164     template <class T>
00165     class SparseSymmetricMatrix : public SparseMatrix<T>
00166     {
00167       public:
00168         virtual ~SparseSymmetricMatrix () {}
00169 
00170         template<class T2>
00171         Vector<T2> operator * (const Vector<T2>& V) const;
00172 
00173         template<class T2>
00174         Vector<T2> Multiply (const Vector<T2>& V ) const;
00175 
00176         template<class T2> void 
00177         Multiply (const Vector<T2>& In, Vector<T2>& Out ) const;
00178 
00179         template<class T2> static int 
00180         Solve (const SparseSymmetricMatrix<T>& M,
00181                const Vector<T2>& b,
00182                const int& iters,
00183                Vector<T2>& solution,
00184                const T2 eps = 1e-8,
00185                const int& reset=1);
00186 
00187         template<class T2> static int 
00188         Solve (const SparseSymmetricMatrix<T>& M,
00189                const Vector<T>& diagonal,
00190                const Vector<T2>& b,
00191                const int& iters,
00192                Vector<T2>& solution,
00193                const T2 eps = 1e-8,
00194                const int& reset=1);
00195     };
00196   }
00197 }
00198 
00199 #include <pcl/surface/impl/poisson/sparse_matrix.hpp>
00200 
00201 #endif /* PCL_POISSON_SPARSE_MATRIX_H_ */
00202 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:58