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