42 #include <pcl/common/bivariate_polynomial.h>
54 template<
typename real>
56 degree(0), parameters(nullptr), gradient_x(nullptr), gradient_y(nullptr)
62 template<
typename real>
64 degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL)
70 template<
typename real>
77 template<
typename real>
void
86 int oldDegree = degree;
88 if (oldDegree != degree)
91 parameters =
new real[getNoOfParameters ()];
93 delete gradient_x; gradient_x =
nullptr;
94 delete gradient_y; gradient_y =
nullptr;
98 template<
typename real>
void
101 delete[] parameters; parameters =
nullptr;
102 delete gradient_x; gradient_x =
nullptr;
103 delete gradient_y; gradient_y =
nullptr;
107 template<
typename real>
void
110 if (
this == &other)
return;
111 if (degree != other.
degree)
115 parameters =
new real[getNoOfParameters ()];
121 gradient_x =
nullptr;
122 gradient_y =
nullptr;
124 else if (!gradient_x)
130 std::copy_n(other.
parameters, getNoOfParameters (), parameters);
140 template<
typename real>
void
143 if (gradient_x!=NULL && !forceRecalc)
return;
145 if (gradient_x == NULL)
147 if (gradient_y == NULL)
150 unsigned int parameterPosDx=0, parameterPosDy=0;
151 for (
int xDegree=degree; xDegree>=0; xDegree--)
153 for (
int yDegree=degree-xDegree; yDegree>=0; yDegree--)
157 gradient_x->parameters[parameterPosDx] = xDegree * parameters[parameterPosDx];
162 gradient_y->parameters[parameterPosDy] = yDegree * parameters[ ( (degree+2-xDegree)* (degree+1-xDegree))/2 -
171 template<
typename real> real
174 unsigned int parametersSize = getNoOfParameters ();
175 real* tmpParameter = ¶meters[parametersSize-1];
176 real tmpX=1.0, tmpY, ret=0;
177 for (
int xDegree=0; xDegree<=degree; xDegree++)
180 for (
int yDegree=0; yDegree<=degree-xDegree; yDegree++)
182 ret += (*tmpParameter)*tmpX*tmpY;
192 template<
typename real>
void
195 calculateGradient ();
196 gradX = gradient_x->getValue (x, y);
197 gradY = gradient_y->getValue (x, y);
201 template<
typename real>
void
203 std::vector<int>& types)
const
211 real x = (
static_cast<real
>(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) /
212 (parameters[1]*parameters[1] -
static_cast<real
>(4)*parameters[0]*parameters[3]),
213 y = (
static_cast<real
>(-2)*parameters[0]*x - parameters[2]) / parameters[1];
215 if (!std::isfinite(x) || !std::isfinite(y))
219 real det_H =
static_cast<real
>(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1];
221 if (det_H >
static_cast<real
>(0))
223 if (parameters[0]+parameters[3] <
static_cast<real
>(0))
228 x_values.push_back(x);
229 y_values.push_back(y);
230 types.push_back(type);
234 std::cerr << __PRETTY_FUNCTION__ <<
" is not implemented for polynomials of degree "<<degree<<
". Sorry.\n";
239 template<
typename real> std::ostream&
244 real currentParameter;
245 for (
int xDegree=p.
degree; xDegree>=0; xDegree--)
247 for (
int yDegree=p.
degree-xDegree; yDegree>=0; yDegree--)
249 currentParameter = *tmpParameter;
252 os << (currentParameter<0.0?
" - ":
" + ");
253 currentParameter = std::abs (currentParameter);
255 os << currentParameter;
277 template<
typename real>
void
280 os.write (
reinterpret_cast<const char*
> (°ree),
sizeof (
int));
281 unsigned int paramCnt = getNoOfParametersFromDegree (this->degree);
282 os.write (
reinterpret_cast<const char*
> (this->parameters), paramCnt *
sizeof (real));
286 template<
typename real>
void
289 std::ofstream fout (filename);
294 template<
typename real>
void
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));
305 template<
typename real>
void
308 std::ifstream fin (filename);
This represents a bivariate polynomial and provides some functionality for it.
~BivariatePolynomialT()
Destructor.
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)