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