kindred/create
1
1
Fork 0
Code Issues 37 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

RE: apply clang format

Browse Source
This commit is contained in:
wmayer
2023-09-02 11:46:46 +02:00
committed by wwmayer
parent 26b1e2bb7f
commit ac15b03c1b
19 changed files with 1465 additions and 1237 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
src/Mod/ReverseEngineering/App/SampleConsensus.cpp
View File

@@ -22,7 +22,7 @@
#include "PreCompiled.h"
#ifndef _PreComp_
# include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#endif
#include <Base/Exception.h>
@@ -32,41 +32,44 @@
#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_cone.h>
# include <pcl/sample_consensus/sac_model_cylinder.h>
# include <pcl/sample_consensus/sac_model_plane.h>
# include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/features/normal_3d.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_cone.h>
#include <pcl/sample_consensus/sac_model_cylinder.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
using namespace std;
using namespace Reen;
using pcl::PointXYZ;
using pcl::PointNormal;
using pcl::PointCloud;
using pcl::PointNormal;
using pcl::PointXYZ;
SampleConsensus::SampleConsensus(SacModel sac, const Points::PointKernel& pts, const std::vector<Base::Vector3d>& nor)
: mySac(sac)
, myPoints(pts)
, myNormals(nor)
{
}
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>);
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))
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->width = int(cloud->points.size());
cloud->height = 1;
cloud->is_dense = true;
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal> ());
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>());
if (mySac == SACMODEL_CONE || mySac == SACMODEL_CYLINDER) {
#if 0
// Create search tree
@@ -83,9 +86,13 @@ double SampleConsensus::perform(std::vector<float>& parameters, std::vector<int>
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))
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
}
@@ -93,48 +100,44 @@ double SampleConsensus::perform(std::vector<float>& parameters, std::vector<int>
// 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");
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);
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
ransac.setDistanceThreshold(.01);
ransac.computeModel();
ransac.getInliers(model);
//ransac.getModel (model);
// ransac.getModel (model);
Eigen::VectorXf model_p_coefficients;
ransac.getModelCoefficients (model_p_coefficients);
for (int i=0; i<model_p_coefficients.size(); i++)
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
#endif// HAVE_PCL_SAMPLE_CONSENSUS