Point Cloud Library (PCL) 1.14.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{
57 setDegree(new_degree);
58}
59
60
61template<typename real>
66
67
68template<typename real>
73
74
75template<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
96template<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
105template<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
138template<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
169template<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
190template<typename real> void
191BivariatePolynomialT<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
199template<typename real> void
200BivariatePolynomialT<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
237template<typename real> std::ostream&
238operator<< (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
275template<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
284template<typename real> void
285BivariatePolynomialT<real>::writeBinary (const char* filename) const
286{
287 std::ofstream fout (filename);
288 writeBinary (fout);
289}
290
291
292template<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
303template<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)