313 lines
8.3 KiB
C++
313 lines
8.3 KiB
C++
/*
|
|
* 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) :
|
|
degree(0), parameters(nullptr), gradient_x(nullptr), gradient_y(nullptr)
|
|
{
|
|
setDegree(new_degree);
|
|
}
|
|
|
|
|
|
template<typename real>
|
|
BivariatePolynomialT<real>::BivariatePolynomialT (const BivariatePolynomialT& other) :
|
|
degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL)
|
|
{
|
|
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!=NULL && !forceRecalc) return;
|
|
|
|
if (gradient_x == NULL)
|
|
gradient_x = new pcl::BivariatePolynomialT<real> (degree-1);
|
|
if (gradient_y == NULL)
|
|
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 = ¶meters[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 = (real(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) /
|
|
(parameters[1]*parameters[1] - real(4)*parameters[0]*parameters[3]),
|
|
y = (real(-2)*parameters[0]*x - parameters[2]) / parameters[1];
|
|
|
|
if (!std::isfinite(x) || !std::isfinite(y))
|
|
return;
|
|
|
|
int type = 2;
|
|
real det_H = real(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1];
|
|
//std::cout << "det(H) = "<<det_H<<"\n";
|
|
if (det_H > real(0)) // Check Hessian determinant
|
|
{
|
|
if (parameters[0]+parameters[3] < 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*> (°ree), 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
|