Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add header guard and copyright info to polynomial_calculations.hpp #2500

Merged
merged 1 commit into from
Oct 2, 2018
Merged

Add header guard and copyright info to polynomial_calculations.hpp #2500

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
89 changes: 66 additions & 23 deletions common/include/pcl/common/impl/polynomial_calculations.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,43 @@
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, 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.
*
*/
#ifndef PCL_POLYNOMIAL_CALCULATIONS_HPP_
#define PCL_POLYNOMIAL_CALCULATIONS_HPP_

////////////////////////////////////

template <typename real>
Expand All @@ -15,7 +55,7 @@ pcl::PolynomialCalculationsT<real>:: ~PolynomialCalculationsT ()
////////////////////////////////////

template <typename real>
inline void
inline void
pcl::PolynomialCalculationsT<real>::Parameters::setZeroValue (real new_zero_value)
{
zero_value = new_zero_value;
Expand All @@ -38,7 +78,7 @@ inline void
{
roots.push_back (-b/a);
}

#if 0
cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
for (unsigned int i=0; i<roots.size (); i++)
Expand Down Expand Up @@ -147,7 +187,7 @@ inline void
alpha2 = alpha*alpha,
alpha3 = alpha2*alpha,
beta2 = beta*beta;

// Value for resubstitution:
double resubValue = b/ (3*a);

Expand Down Expand Up @@ -195,7 +235,7 @@ inline void
roots.push_back (-tmp1*cos (tmp2 + M_PI/3.0) - resubValue);
roots.push_back (-tmp1*cos (tmp2 - M_PI/3.0) - resubValue);
}

#if 0
cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
for (unsigned int i=0; i<roots.size (); i++)
Expand Down Expand Up @@ -227,7 +267,7 @@ inline void
//cout << "Highest order element is 0 => Calling solveCubicEquation.\n";
solveCubicEquation (b, c, d, e, roots);
return;
}
}

if (isNearlyZero (e))
{
Expand All @@ -239,7 +279,7 @@ inline void
if (!isNearlyZero (tmpRoots[i]))
roots.push_back (tmpRoots[i]);
return;
}
}

double root1, root2, root3, root4,
a2 = a*a,
Expand All @@ -252,12 +292,12 @@ inline void
beta = (b3/ (8.0*a3)) - ( (b*c)/ (2.0*a2)) + (d/a),
gamma = ( (-3.0*b4)/ (256.0*a4)) + ( (c*b2)/ (16.0*a3)) - ( (b*d)/ (4.0*a2)) + (e/a),
alpha2 = alpha*alpha;

// Value for resubstitution:
double resubValue = b/ (4*a);

//cout << "Trying to solve y^4 + "<<alpha<<"y^2 + "<<beta<<"y + "<<gamma<<"\n";

if (isNearlyZero (beta))
{ // y^4 + alpha*y^2 + gamma\n";
//cout << "Using beta=0 condition\n";
Expand Down Expand Up @@ -300,7 +340,7 @@ inline void
y += p/ (3.0*u);

double w = alpha + 2.0*y;

if (w > 0)
{
w = sqrt (w);
Expand All @@ -317,7 +357,7 @@ inline void

double tmp1 = - (3.0*alpha + 2.0*y + 2.0* (beta/w)),
tmp2 = - (3.0*alpha + 2.0*y - 2.0* (beta/w));

if (tmp1 > 0)
{
tmp1 = sqrt (tmp1);
Expand Down Expand Up @@ -345,10 +385,10 @@ inline void
root3 = - (b/ (4.0*a)) - 0.5*w;
roots.push_back (root3);
}

//cout << "Test: " << alpha<<", "<<beta<<", "<<gamma<<", "<<p<<", "<<q<<", "<<u <<", "<<y<<", "<<w<<"\n";
}

#if 0
cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
for (unsigned int i=0; i<roots.size (); i++)
Expand Down Expand Up @@ -393,19 +433,19 @@ inline bool

//cout << "Searching for the "<<parameters_size<<" parameters for the bivariate polynom of degree "
// << polynomial_degree<<" using "<<samplePoints.size ()<<" points.\n";

if (parameters_size > samplePoints.size ()) // Too many parameters for this number of equations (points)?
{
return false;
return false;
// Reduce degree of polynomial
//polynomial_degree = (unsigned int) (0.5f* (std::sqrt (8*samplePoints.size ()+1) - 3));
//parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
//cout << "Not enough points, so degree of polynomial was decreased to "<<polynomial_degree
// << " ("<<samplePoints.size ()<<" points => "<<parameters_size<<" parameters)\n";
}

ret.setDegree (polynomial_degree);

//double coeffStuffStartTime=-get_time ();
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A (parameters_size, parameters_size);
A.setZero();
Expand Down Expand Up @@ -434,20 +474,20 @@ inline bool
}
tmpX *= currentX;
}

real* APtr = &A(0,0);
real* bPtr = &b[0];
real* tmpCPtr1=tmpC;
for (unsigned int i=0; i<parameters_size; ++i)
{
* (bPtr++) += currentZ * *tmpCPtr1;

real* tmpCPtr2=tmpC;
for (unsigned int j=0; j<parameters_size; ++j)
{
* (APtr++) += *tmpCPtr1 * * (tmpCPtr2++);
}

++tmpCPtr1;
}
//A += DMatrix<real>::outProd (tmpC);
Expand Down Expand Up @@ -489,7 +529,7 @@ inline bool
//}
//cout << "Calculating matrix A and vector b (size "<<b.size ()<<") from "<<samplePoints.size ()<<" points took "
//<< (coeffStuffStartTime+get_time ())*1000<<"ms.\n";

Eigen::Matrix<real, Eigen::Dynamic, 1> parameters;
//double choleskyStartTime=-get_time ();
//parameters = A.choleskySolve (b);
Expand All @@ -500,21 +540,24 @@ inline bool
//cout << "Inverse took "<< (invStartTime+get_time ())*1000<<"ms.\n";

//cout << PVARC (A)<<PVARC (b)<<PVARN (parameters);

real inversionCheckResult = (A*parameters - b).norm ();
if (inversionCheckResult > 1e-5)
{
//cout << "Inversion result: "<< inversionCheckResult<<" for matrix "<<A<<"\n";
return false;
}

for (unsigned int i=0; i<parameters_size; i++)
ret.parameters[i] = parameters[i];

//cout << "Resulting polynomial is "<<ret<<"\n";

//Test of gradient: ret.calculateGradient ();

delete [] tmpC;
return true;
}

#endif // PCL_POLYNOMIAL_CALCULATIONS_HPP_