Point Cloud Library (PCL)  1.9.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 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
43 template<typename real>
45  degree(0), parameters(nullptr), gradient_x(nullptr), gradient_y(nullptr)
46 {
47  setDegree(new_degree);
48 }
49 
50 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51 template<typename real>
53  degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL)
54 {
55  deepCopy (other);
56 }
57 
58 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
59 template<typename real>
61 {
62  memoryCleanUp ();
63 }
64 
65 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
66 template<typename real> void
68 {
69  if (newDegree <= 0)
70  {
71  degree = -1;
72  memoryCleanUp();
73  return;
74  }
75  int oldDegree = degree;
76  degree = newDegree;
77  if (oldDegree != degree)
78  {
79  delete[] parameters;
80  parameters = new real[getNoOfParameters ()];
81  }
82  delete gradient_x; gradient_x = nullptr;
83  delete gradient_y; gradient_y = nullptr;
84 }
85 
86 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
87 template<typename real> void
89 {
90  delete[] parameters; parameters = nullptr;
91  delete gradient_x; gradient_x = nullptr;
92  delete gradient_y; gradient_y = nullptr;
93 }
94 
95 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
96 template<typename real> void
98 {
99  if (this == &other) return;
100  if (degree != other.degree)
101  {
102  memoryCleanUp ();
103  degree = other.degree;
104  parameters = new real[getNoOfParameters ()];
105  }
106  if (other.gradient_x == NULL)
107  {
108  delete gradient_x; gradient_x=NULL;
109  delete gradient_y; gradient_y=NULL;
110  }
111  else if (gradient_x==NULL)
112  {
115  }
116  real* tmpParameters1 = parameters;
117  const real* tmpParameters2 = other.parameters;
118  unsigned int noOfParameters = getNoOfParameters ();
119  for (unsigned int i=0; i<noOfParameters; i++)
120  *tmpParameters1++ = *tmpParameters2++;
121 
122  if (other.gradient_x != NULL)
123  {
124  gradient_x->deepCopy (*other.gradient_x);
125  gradient_y->deepCopy (*other.gradient_y);
126  }
127 }
128 
129 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
130 template<typename real> void
132 {
133  if (gradient_x!=NULL && !forceRecalc) return;
134 
135  if (gradient_x == NULL)
137  if (gradient_y == NULL)
139 
140  unsigned int parameterPosDx=0, parameterPosDy=0;
141  for (int xDegree=degree; xDegree>=0; xDegree--)
142  {
143  for (int yDegree=degree-xDegree; yDegree>=0; yDegree--)
144  {
145  if (xDegree > 0)
146  {
147  gradient_x->parameters[parameterPosDx] = xDegree * parameters[parameterPosDx];
148  parameterPosDx++;
149  }
150  if (yDegree > 0)
151  {
152  gradient_y->parameters[parameterPosDy] = yDegree * parameters[ ( (degree+2-xDegree)* (degree+1-xDegree))/2 -
153  yDegree - 1];
154  parameterPosDy++;
155  }
156  }
157  }
158 }
159 
160 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
161 template<typename real> real
163 {
164  unsigned int parametersSize = getNoOfParameters ();
165  real* tmpParameter = &parameters[parametersSize-1];
166  real tmpX=1.0, tmpY, ret=0;
167  for (int xDegree=0; xDegree<=degree; xDegree++)
168  {
169  tmpY = 1.0;
170  for (int yDegree=0; yDegree<=degree-xDegree; yDegree++)
171  {
172  ret += (*tmpParameter)*tmpX*tmpY;
173  tmpY *= y;
174  tmpParameter--;
175  }
176  tmpX *= x;
177  }
178  return ret;
179 }
180 
181 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
182 template<typename real> void
183 pcl::BivariatePolynomialT<real>::getValueOfGradient (real x, real y, real& gradX, real& gradY)
184 {
186  gradX = gradient_x->getValue (x, y);
187  gradY = gradient_y->getValue (x, y);
188 }
189 
190 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
191 template<typename real> void
192 pcl::BivariatePolynomialT<real>::findCriticalPoints (std::vector<real>& x_values, std::vector<real>& y_values,
193  std::vector<int>& types) const
194 {
195  x_values.clear ();
196  y_values.clear ();
197  types.clear ();
198 
199  if (degree == 2)
200  {
201  real x = (real(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) /
202  (parameters[1]*parameters[1] - real(4)*parameters[0]*parameters[3]),
203  y = (real(-2)*parameters[0]*x - parameters[2]) / parameters[1];
204 
205  if (!std::isfinite(x) || !std::isfinite(y))
206  return;
207 
208  int type = 2;
209  real det_H = real(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1];
210  //std::cout << "det(H) = "<<det_H<<"\n";
211  if (det_H > real(0)) // Check Hessian determinant
212  {
213  if (parameters[0]+parameters[3] < real(0)) // Check Hessian trace
214  type = 0;
215  else
216  type = 1;
217  }
218  x_values.push_back(x);
219  y_values.push_back(y);
220  types.push_back(type);
221  }
222  else
223  {
224  std::cerr << __PRETTY_FUNCTION__ << " is not implemented for polynomials of degree "<<degree<<". Sorry.\n";
225  }
226 }
227 
228 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
229 template<typename real> std::ostream&
230 pcl::operator<< (std::ostream& os, const pcl::BivariatePolynomialT<real>& p)
231 {
232  real* tmpParameter = p.parameters;
233  bool first = true;
234  real currentParameter;
235  for (int xDegree=p.degree; xDegree>=0; xDegree--)
236  {
237  for (int yDegree=p.degree-xDegree; yDegree>=0; yDegree--)
238  {
239  currentParameter = *tmpParameter;
240  if (!first)
241  {
242  os << (currentParameter<0.0?" - ":" + ");
243  currentParameter = fabs (currentParameter);
244  }
245  os << currentParameter;
246  if (xDegree>0)
247  {
248  os << "x";
249  if (xDegree>1)
250  os<<"^"<<xDegree;
251  }
252  if (yDegree>0)
253  {
254  os << "y";
255  if (yDegree>1)
256  os<<"^"<<yDegree;
257  }
258 
259  first = false;
260  tmpParameter++;
261  }
262  }
263  return (os);
264 }
265 
266 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
267 template<typename real> void
269 {
270  os.write (reinterpret_cast<char*> (&degree), sizeof (int));
271  unsigned int paramCnt = getNoOfParametersFromDegree (this->degree);
272  os.write (reinterpret_cast<char*> (this->parameters), paramCnt * sizeof (real));
273 }
274 
275 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
276 template<typename real> void
278 {
279  std::ofstream fout (filename);
280  writeBinary (fout);
281 }
282 
283 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
284 template<typename real> void
286 {
287  memoryCleanUp ();
288  os.read (reinterpret_cast<char*> (&this->degree), sizeof (int));
289  unsigned int paramCnt = getNoOfParametersFromDegree (this->degree);
290  parameters = new real[paramCnt];
291  os.read (reinterpret_cast<char*> (&(*this->parameters)), paramCnt * sizeof (real));
292 }
293 
294 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
295 template<typename real> void
297 {
298  std::ifstream fin (filename);
299  readBinary (fin);
300 }
301 
302 #endif
303 
This represents a bivariate polynomial and provides some functionality for it.
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.
BivariatePolynomialT< real > * gradient_y
unsigned int getNoOfParameters() const
How many parameters has a bivariate polynomial with this degree.
void findCriticalPoints(std::vector< real > &x_values, std::vector< real > &y_values, std::vector< int > &types) const
Returns critical points of the polynomial.
void writeBinary(std::ostream &os) const
write as binary to a stream
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
real getValue(real x, real y) const
Calculate the value of the polynomial at the given point.
static unsigned int getNoOfParametersFromDegree(int n)
How many parameters has a bivariate polynomial of the given degree.
void getValueOfGradient(real x, real y, real &gradX, real &gradY)
Calculate the value of the gradient at the given point.