79 lines
3.3 KiB
C++
79 lines
3.3 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/sample_consensus/ransac.h>
|
|
#include <pcl/sample_consensus/sac_model_plane.h>
|
|
|
|
using namespace std;
|
|
using namespace Reen;
|
|
using pcl::PointXYZ;
|
|
using pcl::PointNormal;
|
|
using pcl::PointCloud;
|
|
|
|
SampleConsensus::SampleConsensus(const Points::PointKernel& pts)
|
|
: myPoints(pts)
|
|
{
|
|
}
|
|
|
|
double SampleConsensus::perform(std::vector<float>& parameters)
|
|
{
|
|
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;
|
|
|
|
// created RandomSampleConsensus object and compute the appropriated model
|
|
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
|
|
model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
|
|
|
|
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
|
|
ransac.setDistanceThreshold (.01);
|
|
ransac.computeModel();
|
|
//ransac.getInliers(inliers);
|
|
//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
|
|
|