240 lines
8.0 KiB
C++
240 lines
8.0 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2015 Werner Mayer <wmayer[at]users.sourceforge.net> *
|
|
* *
|
|
* This file is part of the FreeCAD CAx development system. *
|
|
* *
|
|
* This library is free software; you can redistribute it and/or *
|
|
* modify it under the terms of the GNU Library General Public *
|
|
* License as published by the Free Software Foundation; either *
|
|
* version 2 of the License, or (at your option) any later version. *
|
|
* *
|
|
* This library is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU Library General Public License for more details. *
|
|
* *
|
|
* You should have received a copy of the GNU Library General Public *
|
|
* License along with this library; see the file COPYING.LIB. If not, *
|
|
* write to the Free Software Foundation, Inc., 59 Temple Place, *
|
|
* Suite 330, Boston, MA 02111-1307, USA *
|
|
* *
|
|
***************************************************************************/
|
|
|
|
|
|
#include "PreCompiled.h"
|
|
#if defined(HAVE_PCL_OPENNURBS)
|
|
#ifndef _PreComp_
|
|
# include <Geom_BSplineSurface.hxx>
|
|
# include <TColgp_Array2OfPnt.hxx>
|
|
# include <TColStd_Array2OfReal.hxx>
|
|
# include <TColStd_Array1OfInteger.hxx>
|
|
# include <TColStd_Array1OfReal.hxx>
|
|
# include <map>
|
|
#endif
|
|
|
|
#include <Mod/Points/App/PointsPy.h>
|
|
|
|
#include "BSplineFitting.h"
|
|
|
|
#include <pcl/pcl_config.h>
|
|
#if PCL_VERSION_COMPARE(>=,1,7,0)
|
|
# include <pcl/point_cloud.h>
|
|
# include <pcl/point_types.h>
|
|
# include <pcl/io/pcd_io.h>
|
|
# include <pcl/surface/on_nurbs/fitting_surface_tdm.h>
|
|
# include <pcl/surface/on_nurbs/fitting_curve_2d_asdm.h>
|
|
#endif
|
|
|
|
using namespace Reen;
|
|
|
|
|
|
BSplineFitting::BSplineFitting(const std::vector<Base::Vector3f>& pts)
|
|
: myPoints(pts)
|
|
, myIterations(10)
|
|
, myOrder(3)
|
|
, myRefinement(4)
|
|
, myInteriorSmoothness(0.2)
|
|
, myInteriorWeight(1.0)
|
|
, myBoundarySmoothness(0.2)
|
|
, myBoundaryWeight(0.0)
|
|
{
|
|
}
|
|
|
|
void BSplineFitting::setIterations(unsigned value)
|
|
{
|
|
myIterations = value;
|
|
}
|
|
|
|
void BSplineFitting::setOrder(unsigned value)
|
|
{
|
|
myOrder = value;
|
|
}
|
|
|
|
void BSplineFitting::setRefinement(unsigned value)
|
|
{
|
|
myRefinement = value;
|
|
}
|
|
|
|
void BSplineFitting::setInteriorSmoothness(double value)
|
|
{
|
|
myInteriorSmoothness = value;
|
|
}
|
|
|
|
void BSplineFitting::setInteriorWeight(double value)
|
|
{
|
|
myInteriorWeight = value;
|
|
}
|
|
|
|
void BSplineFitting::setBoundarySmoothness(double value)
|
|
{
|
|
myBoundarySmoothness = value;
|
|
}
|
|
|
|
void BSplineFitting::setBoundaryWeight(double value)
|
|
{
|
|
myBoundaryWeight = value;
|
|
}
|
|
|
|
Handle(Geom_BSplineSurface) BSplineFitting::perform()
|
|
{
|
|
#if PCL_VERSION_COMPARE(>=,1,7,0)
|
|
pcl::on_nurbs::NurbsDataSurface data;
|
|
for (std::vector<Base::Vector3f>::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
|
|
if (!pcl_isnan (it->x) && !pcl_isnan (it->y) && !pcl_isnan (it->z))
|
|
data.interior.push_back (Eigen::Vector3d (it->x, it->y, it->z));
|
|
}
|
|
|
|
|
|
// fit B-spline surface
|
|
//
|
|
|
|
pcl::on_nurbs::FittingSurface::Parameter params;
|
|
params.interior_smoothness = myInteriorSmoothness;
|
|
params.interior_weight = myInteriorWeight;
|
|
params.boundary_smoothness = myBoundarySmoothness;
|
|
params.boundary_weight = myBoundaryWeight;
|
|
|
|
// initialize
|
|
ON_NurbsSurface nurbs = pcl::on_nurbs::FittingSurface::initNurbsPCABoundingBox (myOrder, &data);
|
|
pcl::on_nurbs::FittingSurface fit (&data, nurbs);
|
|
// fit.setQuiet (false); // enable/disable debug output
|
|
|
|
// surface refinement
|
|
for (unsigned i = 0; i < myRefinement; i++) {
|
|
fit.refine(0);
|
|
fit.refine(1);
|
|
fit.assemble(params);
|
|
fit.solve();
|
|
}
|
|
|
|
// surface fitting with final refinement level
|
|
for (unsigned i = 0; i < myIterations; i++) {
|
|
fit.assemble(params);
|
|
fit.solve();
|
|
}
|
|
|
|
// fit B-spline curve
|
|
#if 0
|
|
// parameters
|
|
pcl::on_nurbs::FittingCurve2dAPDM::FitParameter curve_params;
|
|
curve_params.addCPsAccuracy = 5e-2;
|
|
curve_params.addCPsIteration = 3;
|
|
curve_params.maxCPs = 200;
|
|
curve_params.accuracy = 1e-3;
|
|
curve_params.iterations = 100;
|
|
|
|
curve_params.param.closest_point_resolution = 0;
|
|
curve_params.param.closest_point_weight = 1.0;
|
|
curve_params.param.closest_point_sigma2 = 0.1;
|
|
curve_params.param.interior_sigma2 = 0.00001;
|
|
curve_params.param.smooth_concavity = 1.0;
|
|
curve_params.param.smoothness = 1.0;
|
|
|
|
// initialisation (circular)
|
|
pcl::on_nurbs::NurbsDataCurve2d curve_data;
|
|
curve_data.interior = data.interior_param;
|
|
curve_data.interior_weight_function.push_back(true);
|
|
ON_NurbsCurve curve_nurbs = pcl::on_nurbs::FittingCurve2dAPDM::initNurbsCurve2D(order, curve_data.interior);
|
|
|
|
// curve fitting
|
|
pcl::on_nurbs::FittingCurve2dASDM curve_fit (&curve_data, curve_nurbs);
|
|
// curve_fit.setQuiet (false); // enable/disable debug output
|
|
curve_fit.fitting (curve_params);
|
|
#endif
|
|
|
|
// u parameters
|
|
int numUKnots = fit.m_nurbs.KnotCount(0);
|
|
int numUPoles = fit.m_nurbs.CVCount(0);
|
|
int uDegree = fit.m_nurbs.Degree(0);
|
|
bool uPeriodic = fit.m_nurbs.IsPeriodic(0) ? true : false;
|
|
std::map<Standard_Real, int> uKnots;
|
|
|
|
// v parameters
|
|
int numVKnots = fit.m_nurbs.KnotCount(1);
|
|
int numVPoles = fit.m_nurbs.CVCount(1);
|
|
int vDegree = fit.m_nurbs.Degree(1);
|
|
bool vPeriodic = fit.m_nurbs.IsPeriodic(1) ? true : false;
|
|
std::map<Standard_Real, int> vKnots;
|
|
|
|
TColgp_Array2OfPnt poles(1, numUPoles, 1, numVPoles);
|
|
TColStd_Array2OfReal weights(1, numUPoles, 1, numVPoles);
|
|
|
|
for (int i=0; i<numUPoles; i++) {
|
|
for (int j=0; j<numVPoles; j++) {
|
|
ON_3dPoint cv;
|
|
fit.m_nurbs.GetCV(i, j, cv);
|
|
poles.SetValue(i+1, j+1, gp_Pnt(cv.x,cv.y,cv.z));
|
|
|
|
Standard_Real weight = fit.m_nurbs.Weight(i, j);
|
|
weights.SetValue(i+1, j+1, weight);
|
|
}
|
|
}
|
|
|
|
uKnots[fit.m_nurbs.SuperfluousKnot(0,0)] = 1;
|
|
uKnots[fit.m_nurbs.SuperfluousKnot(0,1)] = 1;
|
|
for (int i=0; i<numUKnots; i++) {
|
|
Standard_Real value = fit.m_nurbs.Knot(0, i);
|
|
std::map<Standard_Real, int>::iterator it = uKnots.find(value);
|
|
if (it == uKnots.end())
|
|
uKnots[value] = 1;
|
|
else
|
|
it->second++;
|
|
}
|
|
|
|
vKnots[fit.m_nurbs.SuperfluousKnot(1,0)] = 1;
|
|
vKnots[fit.m_nurbs.SuperfluousKnot(1,1)] = 1;
|
|
for (int i=0; i<numVKnots; i++) {
|
|
Standard_Real value = fit.m_nurbs.Knot(1, i);
|
|
std::map<Standard_Real, int>::iterator it = vKnots.find(value);
|
|
if (it == vKnots.end())
|
|
vKnots[value] = 1;
|
|
else
|
|
it->second++;
|
|
}
|
|
|
|
TColStd_Array1OfReal uKnotArray(1,uKnots.size());
|
|
TColStd_Array1OfInteger uMultArray(1,uKnots.size());
|
|
int index = 1;
|
|
for (std::map<Standard_Real, int>::iterator it = uKnots.begin(); it != uKnots.end(); ++it, index++) {
|
|
uKnotArray.SetValue(index, it->first);
|
|
uMultArray.SetValue(index, it->second);
|
|
}
|
|
|
|
TColStd_Array1OfReal vKnotArray(1,vKnots.size());
|
|
TColStd_Array1OfInteger vMultArray(1,vKnots.size());
|
|
index = 1;
|
|
for (std::map<Standard_Real, int>::iterator it = vKnots.begin(); it != vKnots.end(); ++it, index++) {
|
|
vKnotArray.SetValue(index, it->first);
|
|
vMultArray.SetValue(index, it->second);
|
|
}
|
|
|
|
Handle_Geom_BSplineSurface spline = new Geom_BSplineSurface(poles,weights,
|
|
uKnotArray, vKnotArray, uMultArray, vMultArray, uDegree, vDegree,
|
|
uPeriodic, vPeriodic);
|
|
return spline;
|
|
#else
|
|
return Handle_Geom_BSplineSurface();
|
|
#endif
|
|
}
|
|
#endif // HAVE_PCL_OPENNURBS
|