Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
51namespace pcl
52{
53
54template<typename real>
56 degree(0), parameters(nullptr), gradient_x(nullptr), gradient_y(nullptr)
57{
58 setDegree(new_degree);
59}
60
61
62template<typename real>
64 degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL)
65{
66 deepCopy (other);
67}
68
69
70template<typename real>
72{
73 memoryCleanUp ();
74}
75
76
77template<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
98template<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
107template<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
140template<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
171template<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
192template<typename real> void
193BivariatePolynomialT<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
201template<typename real> void
202BivariatePolynomialT<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
239template<typename real> std::ostream&
240operator<< (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
277template<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
286template<typename real> void
287BivariatePolynomialT<real>::writeBinary (const char* filename) const
288{
289 std::ofstream fout (filename);
290 writeBinary (fout);
291}
292
293
294template<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
305template<typename real> void
307{
308 std::ifstream fin (filename);
309 readBinary (fin);
310}
311
312} // 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.
void readBinary(std::istream &os)
read binary from a stream
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 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)