1 /*
2  * Software License Agreement (BSD License)
3  *
4  *  Copyright (c) 2012-, Open Perception, Inc.
5  *  All rights reserved.
6  *
7  *  Redistribution and use in source and binary forms, with or without
8  *  modification, are permitted provided that the following conditions
9  *  are met:
10  *
11  *   * Redistributions of source code must retain the above copyright
12  *     notice, this list of conditions and the following disclaimer.
13  *   * Redistributions in binary form must reproduce the above
14  *     copyright notice, this list of conditions and the following
15  *     disclaimer in the documentation and/or other materials provided
16  *     with the distribution.
17  *   * Neither the name of the copyright holder(s) nor the names of its
18  *     contributors may be used to endorse or promote products derived
19  *     from this software without specific prior written permission.
20  *
21  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  *  POSSIBILITY OF SUCH DAMAGE.
33  *
34  *
35  *
36  */
37 
38 #pragma once
39 
40 #undef Success
41 #include <pcl/memory.h>
42 #include <pcl/pcl_macros.h>
43 #include <pcl/surface/on_nurbs/sparse_mat.h>
44 
45 namespace pcl
46 {
47   namespace on_nurbs
48   {
49 
50     /** \brief Solving the linear system of equations using Eigen or UmfPack.
51      * (can be defined in on_nurbs.cmake)*/
52     class NurbsSolve
53     {
54     public:
55       /** \brief Empty constructor */
NurbsSolve()56       NurbsSolve () :
57         m_quiet (true)
58       {
59       }
60 
61       /** \brief Assign size and dimension (2D, 3D) of system of equations. */
62       void
63       assign (unsigned rows, unsigned cols, unsigned dims);
64 
65       /** \brief Set value for system matrix K (stiffness matrix, basis functions) */
66       void
67       K (unsigned i, unsigned j, double v);
68       /** \brief Set value for state vector x (control points) */
69       void
70       x (unsigned i, unsigned j, double v);
71       /** \brief Set value for target vector f (force vector) */
72       void
73       f (unsigned i, unsigned j, double v);
74 
75       /** \brief Get value for system matrix K (stiffness matrix, basis functions) */
76       double
77       K (unsigned i, unsigned j);
78       /** \brief Get value for state vector x (control points) */
79       double
80       x (unsigned i, unsigned j);
81       /** \brief Get value for target vector f (force vector) */
82       double
83       f (unsigned i, unsigned j);
84 
85       /** \brief Resize target vector f (force vector) */
86       void
87       resize (unsigned rows);
88 
89       /** \brief Print system matrix K (stiffness matrix, basis functions) */
90       void
91       printK ();
92       /** \brief Print state vector x (control points) */
93       void
94       printX ();
95       /** \brief Print target vector f (force vector) */
96       void
97       printF ();
98 
99       /** \brief Solves the system of equations with respect to x.
100        *  - Using UmfPack incredibly speeds up this function.      */
101       bool
102       solve ();
103 
104       /** \brief Compute the difference between solution K*x and target f */
105       Eigen::MatrixXd
106       diff ();
107 
108       /** \brief Enable/Disable debug outputs in console. */
109       inline void
setQuiet(bool val)110       setQuiet (bool val)
111       {
112         m_quiet = val;
113       }
114 
115       /** \brief get size of system */
116       inline void
getSize(unsigned & rows,unsigned & cols,unsigned & dims)117       getSize (unsigned &rows, unsigned &cols, unsigned &dims)
118       {
119         rows = static_cast<unsigned>(m_feig.rows());
120         cols = static_cast<unsigned>(m_xeig.rows());
121         dims = static_cast<unsigned>(m_feig.cols());
122       }
123 
124     private:
125       bool m_quiet;
126       SparseMat m_Ksparse;
127       Eigen::MatrixXd m_Keig;
128       Eigen::MatrixXd m_xeig;
129       Eigen::MatrixXd m_feig;
130 
131     public:
132       PCL_MAKE_ALIGNED_OPERATOR_NEW
133     };
134 
135   }
136 }
137