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