138 lines
5.4 KiB
C++
138 lines
5.4 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2016 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"
|
|
|
|
#include "SampleConsensus.h"
|
|
#include <Mod/Points/App/Points.h>
|
|
#include <Base/Exception.h>
|
|
#include <boost/math/special_functions/fpclassify.hpp>
|
|
|
|
#if defined(HAVE_PCL_SAMPLE_CONSENSUS)
|
|
#include <pcl/point_types.h>
|
|
#include <pcl/features/normal_3d.h>
|
|
#include <pcl/sample_consensus/ransac.h>
|
|
#include <pcl/sample_consensus/sac_model_plane.h>
|
|
#include <pcl/sample_consensus/sac_model_sphere.h>
|
|
#include <pcl/sample_consensus/sac_model_cylinder.h>
|
|
#include <pcl/sample_consensus/sac_model_cone.h>
|
|
|
|
using namespace std;
|
|
using namespace Reen;
|
|
using pcl::PointXYZ;
|
|
using pcl::PointNormal;
|
|
using pcl::PointCloud;
|
|
|
|
SampleConsensus::SampleConsensus(SacModel sac, const Points::PointKernel& pts, const std::vector<Base::Vector3d>& nor)
|
|
: mySac(sac)
|
|
, myPoints(pts)
|
|
, myNormals(nor)
|
|
{
|
|
}
|
|
|
|
double SampleConsensus::perform(std::vector<float>& parameters, std::vector<int>& model)
|
|
{
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
|
|
cloud->reserve(myPoints.size());
|
|
for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
|
|
if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z))
|
|
cloud->push_back(pcl::PointXYZ(it->x, it->y, it->z));
|
|
}
|
|
|
|
cloud->width = int (cloud->points.size ());
|
|
cloud->height = 1;
|
|
cloud->is_dense = true;
|
|
|
|
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal> ());
|
|
if (mySac == SACMODEL_CONE || mySac == SACMODEL_CYLINDER) {
|
|
#if 0
|
|
// Create search tree
|
|
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
|
|
tree.reset (new pcl::search::KdTree<PointXYZ> (false));
|
|
tree->setInputCloud (cloud);
|
|
|
|
// Normal estimation
|
|
int ksearch = 10;
|
|
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
|
|
n.setInputCloud (cloud);
|
|
n.setSearchMethod (tree);
|
|
n.setKSearch (ksearch);
|
|
n.compute (*normals);
|
|
#else
|
|
normals->reserve(myNormals.size());
|
|
for (std::vector<Base::Vector3d>::const_iterator it = myNormals.begin(); it != myNormals.end(); ++it) {
|
|
if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z))
|
|
normals->push_back(pcl::Normal(it->x, it->y, it->z));
|
|
}
|
|
#endif
|
|
}
|
|
|
|
// created RandomSampleConsensus object and compute the appropriated model
|
|
pcl::SampleConsensusModel<pcl::PointXYZ>::Ptr model_p;
|
|
switch (mySac) {
|
|
case SACMODEL_PLANE:
|
|
{
|
|
model_p.reset(new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
|
|
break;
|
|
}
|
|
case SACMODEL_SPHERE:
|
|
{
|
|
model_p.reset(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud));
|
|
break;
|
|
}
|
|
case SACMODEL_CONE:
|
|
{
|
|
pcl::SampleConsensusModelCone<pcl::PointXYZ, pcl::Normal>::Ptr model_c
|
|
(new pcl::SampleConsensusModelCone<pcl::PointXYZ, pcl::Normal> (cloud));
|
|
model_c->setInputNormals(normals);
|
|
model_p = model_c;
|
|
break;
|
|
}
|
|
case SACMODEL_CYLINDER:
|
|
{
|
|
pcl::SampleConsensusModelCylinder<pcl::PointXYZ, pcl::Normal>::Ptr model_c
|
|
(new pcl::SampleConsensusModelCylinder<pcl::PointXYZ, pcl::Normal> (cloud));
|
|
model_c->setInputNormals(normals);
|
|
model_p = model_c;
|
|
break;
|
|
}
|
|
default:
|
|
throw Base::RuntimeError("Unsupported SAC model");
|
|
}
|
|
|
|
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
|
|
ransac.setDistanceThreshold (.01);
|
|
ransac.computeModel();
|
|
ransac.getInliers(model);
|
|
//ransac.getModel (model);
|
|
Eigen::VectorXf model_p_coefficients;
|
|
ransac.getModelCoefficients (model_p_coefficients);
|
|
for (int i=0; i<model_p_coefficients.size(); i++)
|
|
parameters.push_back(model_p_coefficients[i]);
|
|
|
|
return ransac.getProbability();
|
|
}
|
|
|
|
#endif // HAVE_PCL_SAMPLE_CONSENSUS
|
|
|