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