Loading...
Searching...
No Matches
bivariate_polynomial.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#pragma once
#include <pcl/common/bivariate_polynomial.h>
#include <algorithm>
#include <cmath>
#include <fstream>
#include <iostream>
#include <vector>
namespace pcl
{
template<typename real>
BivariatePolynomialT<real>::BivariatePolynomialT (int new_degree)
{
setDegree(new_degree);
}
template<typename real>
BivariatePolynomialT<real>::BivariatePolynomialT (const BivariatePolynomialT& other)
{
deepCopy (other);
}
template<typename real>
BivariatePolynomialT<real>::~BivariatePolynomialT ()
{
memoryCleanUp ();
}
template<typename real> void
BivariatePolynomialT<real>::setDegree (int newDegree)
{
if (newDegree <= 0)
{
degree = -1;
memoryCleanUp();
return;
}
int oldDegree = degree;
degree = newDegree;
if (oldDegree != degree)
{
delete[] parameters;
parameters = new real[getNoOfParameters ()];
}
delete gradient_x; gradient_x = nullptr;
delete gradient_y; gradient_y = nullptr;
}
template<typename real> void
BivariatePolynomialT<real>::memoryCleanUp ()
{
delete[] parameters; parameters = nullptr;
delete gradient_x; gradient_x = nullptr;
delete gradient_y; gradient_y = nullptr;
}
template<typename real> void
BivariatePolynomialT<real>::deepCopy (const pcl::BivariatePolynomialT<real>& other)
{
if (this == &other) return;
if (degree != other.degree)
{
memoryCleanUp ();
degree = other.degree;
parameters = new real[getNoOfParameters ()];
}
if (!other.gradient_x)
{
delete gradient_x;
delete gradient_y;
gradient_x = nullptr;
gradient_y = nullptr;
}
else if (!gradient_x)
{
gradient_x = new pcl::BivariatePolynomialT<real> ();
gradient_y = new pcl::BivariatePolynomialT<real> ();
}
std::copy_n(other.parameters, getNoOfParameters (), parameters);
if (other.gradient_x != nullptr)
{
gradient_x->deepCopy (*other.gradient_x);
gradient_y->deepCopy (*other.gradient_y);
}
}
template<typename real> void
BivariatePolynomialT<real>::calculateGradient (bool forceRecalc)
{
if (gradient_x!=nullptr && !forceRecalc) return;
if (gradient_x == nullptr)
gradient_x = new pcl::BivariatePolynomialT<real> (degree-1);
if (gradient_y == nullptr)
gradient_y = new pcl::BivariatePolynomialT<real> (degree-1);
unsigned int parameterPosDx=0, parameterPosDy=0;
for (int xDegree=degree; xDegree>=0; xDegree--)
{
for (int yDegree=degree-xDegree; yDegree>=0; yDegree--)
{
if (xDegree > 0)
{
gradient_x->parameters[parameterPosDx] = xDegree * parameters[parameterPosDx];
parameterPosDx++;
}
if (yDegree > 0)
{
gradient_y->parameters[parameterPosDy] = yDegree * parameters[ ( (degree+2-xDegree)* (degree+1-xDegree))/2 -
yDegree - 1];
parameterPosDy++;
}
}
}
}
template<typename real> real
BivariatePolynomialT<real>::getValue (real x, real y) const
{
unsigned int parametersSize = getNoOfParameters ();
real* tmpParameter = &parameters[parametersSize-1];
real tmpX=1.0, tmpY, ret=0;
for (int xDegree=0; xDegree<=degree; xDegree++)
{
tmpY = 1.0;
for (int yDegree=0; yDegree<=degree-xDegree; yDegree++)
{
ret += (*tmpParameter)*tmpX*tmpY;
tmpY *= y;
tmpParameter--;
}
tmpX *= x;
}
return ret;
}
template<typename real> void
BivariatePolynomialT<real>::getValueOfGradient (real x, real y, real& gradX, real& gradY)
{
calculateGradient ();
gradX = gradient_x->getValue (x, y);
gradY = gradient_y->getValue (x, y);
}
template<typename real> void
BivariatePolynomialT<real>::findCriticalPoints (std::vector<real>& x_values, std::vector<real>& y_values,
std::vector<int>& types) const
{
x_values.clear ();
y_values.clear ();
types.clear ();
if (degree == 2)
{
real x = (static_cast<real>(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) /
(parameters[1]*parameters[1] - static_cast<real>(4)*parameters[0]*parameters[3]),
y = (static_cast<real>(-2)*parameters[0]*x - parameters[2]) / parameters[1];
if (!std::isfinite(x) || !std::isfinite(y))
return;
int type = 2;
real det_H = static_cast<real>(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1];
//std::cout << "det(H) = "<<det_H<<"\n";
if (det_H > static_cast<real>(0)) // Check Hessian determinant
{
if (parameters[0]+parameters[3] < static_cast<real>(0)) // Check Hessian trace
type = 0;
else
type = 1;
}
x_values.push_back(x);
y_values.push_back(y);
types.push_back(type);
}
else
{
std::cerr << __PRETTY_FUNCTION__ << " is not implemented for polynomials of degree "<<degree<<". Sorry.\n";
}
}
template<typename real> std::ostream&
operator<< (std::ostream& os, const pcl::BivariatePolynomialT<real>& p)
{
real* tmpParameter = p.parameters;
bool first = true;
real currentParameter;
for (int xDegree=p.degree; xDegree>=0; xDegree--)
{
for (int yDegree=p.degree-xDegree; yDegree>=0; yDegree--)
{
currentParameter = *tmpParameter;
if (!first)
{
os << (currentParameter<0.0?" - ":" + ");
currentParameter = std::abs (currentParameter);
}
os << currentParameter;
if (xDegree>0)
{
os << "x";
if (xDegree>1)
os<<"^"<<xDegree;
}
if (yDegree>0)
{
os << "y";
if (yDegree>1)
os<<"^"<<yDegree;
}
first = false;
tmpParameter++;
}
}
return (os);
}
template<typename real> void
BivariatePolynomialT<real>::writeBinary (std::ostream& os) const
{
os.write (reinterpret_cast<const char*> (&degree), sizeof (int));
unsigned int paramCnt = getNoOfParametersFromDegree (this->degree);
os.write (reinterpret_cast<const char*> (this->parameters), paramCnt * sizeof (real));
}
template<typename real> void
BivariatePolynomialT<real>::writeBinary (const char* filename) const
{
std::ofstream fout (filename);
writeBinary (fout);
}
template<typename real> void
BivariatePolynomialT<real>::readBinary (std::istream& os)
{
memoryCleanUp ();
os.read (reinterpret_cast<char*> (&this->degree), sizeof (int));
unsigned int paramCnt = getNoOfParametersFromDegree (this->degree);
parameters = new real[paramCnt];
os.read (reinterpret_cast<char*> (&(*this->parameters)), paramCnt * sizeof (real));
}
template<typename real> void
BivariatePolynomialT<real>::readBinary (const char* filename)
{
std::ifstream fin (filename);
readBinary (fin);
}
} // namespace pcl
pcl::BivariatePolynomialT
This represents a bivariate polynomial and provides some functionality for it.
Definition bivariate_polynomial.h:54
pcl::BivariatePolynomialT::~BivariatePolynomialT
~BivariatePolynomialT()
Destructor.
Definition bivariate_polynomial.hpp:69
pcl::BivariatePolynomialT::deepCopy
void deepCopy(const BivariatePolynomialT< real > &other)
Create a deep copy of the given polynomial.
Definition bivariate_polynomial.hpp:106
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:200
pcl::BivariatePolynomialT::BivariatePolynomialT
BivariatePolynomialT(int new_degree=0)
Constructor.
Definition bivariate_polynomial.hpp:55
pcl::BivariatePolynomialT::memoryCleanUp
void memoryCleanUp()
Delete all members.
Definition bivariate_polynomial.hpp:97
pcl::BivariatePolynomialT::gradient_y
BivariatePolynomialT< real > * gradient_y
Definition bivariate_polynomial.h:120
pcl::BivariatePolynomialT::readBinary
void readBinary(std::istream &os)
read binary from a stream
Definition bivariate_polynomial.hpp:293
pcl::BivariatePolynomialT::parameters
real * parameters
Definition bivariate_polynomial.h:118
pcl::BivariatePolynomialT::writeBinary
void writeBinary(std::ostream &os) const
write as binary to a stream
Definition bivariate_polynomial.hpp:276
pcl::BivariatePolynomialT::getValue
real getValue(real x, real y) const
Calculate the value of the polynomial at the given point.
Definition bivariate_polynomial.hpp:170
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:139
pcl::BivariatePolynomialT::degree
int degree
Definition bivariate_polynomial.h:117
pcl::BivariatePolynomialT::setDegree
void setDegree(int new_degree)
Initialize members to default values.
Definition bivariate_polynomial.hpp:76
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:191
pcl
Definition convolution.h:46
pcl::operator<<
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
Definition bivariate_polynomial.hpp:238