Point Cloud Library (PCL)  1.10.1-dev
bivariate_polynomial.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 #ifndef BIVARIATE_POLYNOMIAL_HPP
40 #define BIVARIATE_POLYNOMIAL_HPP
41 
42 #include <algorithm>
43 #include <cmath>
44 #include <fstream>
45 #include <iostream>
46 #include <vector>
47 
48 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
49 template<typename real>
51  degree(0), parameters(nullptr), gradient_x(nullptr), gradient_y(nullptr)
52 {
53  setDegree(new_degree);
54 }
55 
56 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
57 template<typename real>
59  degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL)
60 {
61  deepCopy (other);
62 }
63 
64 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
65 template<typename real>
67 {
68  memoryCleanUp ();
69 }
70 
71 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
72 template<typename real> void
74 {
75  if (newDegree <= 0)
76  {
77  degree = -1;
78  memoryCleanUp();
79  return;
80  }
81  int oldDegree = degree;
82  degree = newDegree;
83  if (oldDegree != degree)
84  {
85  delete[] parameters;
86  parameters = new real[getNoOfParameters ()];
87  }
88  delete gradient_x; gradient_x = nullptr;
89  delete gradient_y; gradient_y = nullptr;
90 }
91 
92 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
93 template<typename real> void
95 {
96  delete[] parameters; parameters = nullptr;
97  delete gradient_x; gradient_x = nullptr;
98  delete gradient_y; gradient_y = nullptr;
99 }
100 
101 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
102 template<typename real> void
104 {
105  if (this == &other) return;
106  if (degree != other.degree)
107  {
108  memoryCleanUp ();
109  degree = other.degree;
110  parameters = new real[getNoOfParameters ()];
111  }
112  if (other.gradient_x == NULL)
113  {
114  delete gradient_x; gradient_x=NULL;
115  delete gradient_y; gradient_y=NULL;
116  }
117  else if (gradient_x==NULL)
118  {
121  }
122 
123  std::copy_n(other.parameters, getNoOfParameters (), parameters);
124 
125  if (other.gradient_x != NULL)
126  {
127  gradient_x->deepCopy (*other.gradient_x);
128  gradient_y->deepCopy (*other.gradient_y);
129  }
130 }
131 
132 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
133 template<typename real> void
135 {
136  if (gradient_x!=NULL && !forceRecalc) return;
137 
138  if (gradient_x == NULL)
140  if (gradient_y == NULL)
142 
143  unsigned int parameterPosDx=0, parameterPosDy=0;
144  for (int xDegree=degree; xDegree>=0; xDegree--)
145  {
146  for (int yDegree=degree-xDegree; yDegree>=0; yDegree--)
147  {
148  if (xDegree > 0)
149  {
150  gradient_x->parameters[parameterPosDx] = xDegree * parameters[parameterPosDx];
151  parameterPosDx++;
152  }
153  if (yDegree > 0)
154  {
155  gradient_y->parameters[parameterPosDy] = yDegree * parameters[ ( (degree+2-xDegree)* (degree+1-xDegree))/2 -
156  yDegree - 1];
157  parameterPosDy++;
158  }
159  }
160  }
161 }
162 
163 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
164 template<typename real> real
166 {
167  unsigned int parametersSize = getNoOfParameters ();
168  real* tmpParameter = &parameters[parametersSize-1];
169  real tmpX=1.0, tmpY, ret=0;
170  for (int xDegree=0; xDegree<=degree; xDegree++)
171  {
172  tmpY = 1.0;
173  for (int yDegree=0; yDegree<=degree-xDegree; yDegree++)
174  {
175  ret += (*tmpParameter)*tmpX*tmpY;
176  tmpY *= y;
177  tmpParameter--;
178  }
179  tmpX *= x;
180  }
181  return ret;
182 }
183 
184 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
185 template<typename real> void
186 pcl::BivariatePolynomialT<real>::getValueOfGradient (real x, real y, real& gradX, real& gradY)
187 {
189  gradX = gradient_x->getValue (x, y);
190  gradY = gradient_y->getValue (x, y);
191 }
192 
193 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
194 template<typename real> void
195 pcl::BivariatePolynomialT<real>::findCriticalPoints (std::vector<real>& x_values, std::vector<real>& y_values,
196  std::vector<int>& types) const
197 {
198  x_values.clear ();
199  y_values.clear ();
200  types.clear ();
201 
202  if (degree == 2)
203  {
204  real x = (real(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) /
205  (parameters[1]*parameters[1] - real(4)*parameters[0]*parameters[3]),
206  y = (real(-2)*parameters[0]*x - parameters[2]) / parameters[1];
207 
208  if (!std::isfinite(x) || !std::isfinite(y))
209  return;
210 
211  int type = 2;
212  real det_H = real(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1];
213  //std::cout << "det(H) = "<<det_H<<"\n";
214  if (det_H > real(0)) // Check Hessian determinant
215  {
216  if (parameters[0]+parameters[3] < real(0)) // Check Hessian trace
217  type = 0;
218  else
219  type = 1;
220  }
221  x_values.push_back(x);
222  y_values.push_back(y);
223  types.push_back(type);
224  }
225  else
226  {
227  std::cerr << __PRETTY_FUNCTION__ << " is not implemented for polynomials of degree "<<degree<<". Sorry.\n";
228  }
229 }
230 
231 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
232 template<typename real> std::ostream&
233 pcl::operator<< (std::ostream& os, const pcl::BivariatePolynomialT<real>& p)
234 {
235  real* tmpParameter = p.parameters;
236  bool first = true;
237  real currentParameter;
238  for (int xDegree=p.degree; xDegree>=0; xDegree--)
239  {
240  for (int yDegree=p.degree-xDegree; yDegree>=0; yDegree--)
241  {
242  currentParameter = *tmpParameter;
243  if (!first)
244  {
245  os << (currentParameter<0.0?" - ":" + ");
246  currentParameter = std::abs (currentParameter);
247  }
248  os << currentParameter;
249  if (xDegree>0)
250  {
251  os << "x";
252  if (xDegree>1)
253  os<<"^"<<xDegree;
254  }
255  if (yDegree>0)
256  {
257  os << "y";
258  if (yDegree>1)
259  os<<"^"<<yDegree;
260  }
261 
262  first = false;
263  tmpParameter++;
264  }
265  }
266  return (os);
267 }
268 
269 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
270 template<typename real> void
272 {
273  os.write (reinterpret_cast<const char*> (&degree), sizeof (int));
274  unsigned int paramCnt = getNoOfParametersFromDegree (this->degree);
275  os.write (reinterpret_cast<const char*> (this->parameters), paramCnt * sizeof (real));
276 }
277 
278 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
279 template<typename real> void
281 {
282  std::ofstream fout (filename);
283  writeBinary (fout);
284 }
285 
286 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
287 template<typename real> void
289 {
290  memoryCleanUp ();
291  os.read (reinterpret_cast<char*> (&this->degree), sizeof (int));
292  unsigned int paramCnt = getNoOfParametersFromDegree (this->degree);
293  parameters = new real[paramCnt];
294  os.read (reinterpret_cast<char*> (&(*this->parameters)), paramCnt * sizeof (real));
295 }
296 
297 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
298 template<typename real> void
300 {
301  std::ifstream fin (filename);
302  readBinary (fin);
303 }
304 
305 #endif
This represents a bivariate polynomial and provides some functionality for it.
void findCriticalPoints(std::vector< real > &x_values, std::vector< real > &y_values, std::vector< int > &types) const
Returns critical points of the polynomial.
void calculateGradient(bool forceRecalc=false)
Calculate the gradient of this polynomial If forceRecalc is false, it will do nothing when the gradie...
BivariatePolynomialT< real > * gradient_x
void memoryCleanUp()
Delete all members.
void deepCopy(const BivariatePolynomialT< real > &other)
Create a deep copy of the given polynomial.
void writeBinary(std::ostream &os) const
write as binary to a stream
BivariatePolynomialT< real > * gradient_y
real getValue(real x, real y) const
Calculate the value of the polynomial at the given point.
void setDegree(int new_degree)
Initialize members to default values.
BivariatePolynomialT(int new_degree=0)
Constructor.
void readBinary(std::istream &os)
read binary from a stream
static unsigned int getNoOfParametersFromDegree(int n)
How many parameters has a bivariate polynomial of the given degree.
unsigned int getNoOfParameters() const
How many parameters has a bivariate polynomial with this degree.
void getValueOfGradient(real x, real y, real &gradX, real &gradY)
Calculate the value of the gradient at the given point.