Skip to content

Commit

Permalink
ReverseEngineering: [skip ci] improve segmentation based on point clouds
Browse files Browse the repository at this point in the history
  • Loading branch information
@wwmayer @berndhahnebach
wwmayer authored and berndhahnebach committed Mar 3, 2020
1 parent 9d6cf32 commit 7464c81
Show file tree
Hide file tree
Showing 3 changed files with 150 additions and 13 deletions.
75 changes: 70 additions & 5 deletions src/Mod/ReverseEngineering/App/AppReverseEngineering.cpp
View file
Expand Up @@ -725,27 +725,92 @@ Mesh.show(m)
}
#endif
#if defined(HAVE_PCL_SAMPLE_CONSENSUS)
/*
import ReverseEngineering as reen
import Points
import Part
p = App.ActiveDocument.Points.Points
data = p.Points
n = reen.normalEstimation(p, 10)
model = reen.sampleConsensus(SacModel="Plane", Points=p)
indices = model["Model"]
param = model["Parameters"]
plane = Part.Plane()
plane.Axis = param[0:3]
plane.Position = -plane.Axis * param[3]
np = Points.Points()
np.addPoints([data[i] for i in indices])
Points.show(np)
# sort in descending order
indices = list(indices)
indices.sort(reverse=True)
# remove points of segment
for i in indices:
del data[i]
del n[i]
p = Points.Points()
p.addPoints(data)
model = reen.sampleConsensus(SacModel="Cylinder", Points=p, Normals=n)
indices = model["Model"]
np = Points.Points()
np.addPoints([data[i] for i in indices])
Points.show(np)
*/
Py::Object sampleConsensus(const Py::Tuple& args, const Py::Dict& kwds)
{
PyObject *pts;
PyObject *vec = nullptr;
const char* sacModelType = nullptr;

static char* kwds_sample[] = {"Points", NULL};
if (!PyArg_ParseTupleAndKeywords(args.ptr(), kwds.ptr(), "O!", kwds_sample,
&(Points::PointsPy::Type), &pts))
static char* kwds_sample[] = {"SacModel", "Points", "Normals", NULL};
if (!PyArg_ParseTupleAndKeywords(args.ptr(), kwds.ptr(), "sO!|O", kwds_sample,
&sacModelType, &(Points::PointsPy::Type), &pts, &vec))
throw Py::Exception();

Points::PointKernel* points = static_cast<Points::PointsPy*>(pts)->getPointKernelPtr();
std::vector<Base::Vector3d> normals;
if (vec) {
Py::Sequence list(vec);
normals.reserve(list.size());
for (Py::Sequence::iterator it = list.begin(); it != list.end(); ++it) {
Base::Vector3d v = Py::Vector(*it).toVector();
normals.push_back(v);
}
}

SampleConsensus::SacModel sacModel = SampleConsensus::SACMODEL_PLANE;
if (sacModelType) {
if (strcmp(sacModelType, "Cylinder") == 0)
sacModel = SampleConsensus::SACMODEL_CYLINDER;
else if (strcmp(sacModelType, "Sphere") == 0)
sacModel = SampleConsensus::SACMODEL_SPHERE;
else if (strcmp(sacModelType, "Cone") == 0)
sacModel = SampleConsensus::SACMODEL_CONE;
}

std::vector<float> parameters;
SampleConsensus sample(*points);
double probability = sample.perform(parameters);
SampleConsensus sample(sacModel, *points, normals);
std::vector<int> model;
double probability = sample.perform(parameters, model);

Py::Dict dict;
Py::Tuple tuple(parameters.size());
for (std::size_t i = 0; i < parameters.size(); i++)
tuple.setItem(i, Py::Float(parameters[i]));
Py::Tuple data(model.size());
for (std::size_t i = 0; i < model.size(); i++)
data.setItem(i, Py::Long(model[i]));
dict.setItem(Py::String("Probability"), Py::Float(probability));
dict.setItem(Py::String("Parameters"), tuple);
dict.setItem(Py::String("Model"), data);

return dict;
}
Expand Down
71 changes: 65 additions & 6 deletions src/Mod/ReverseEngineering/App/SampleConsensus.cpp
View file
Expand Up @@ -30,21 +30,27 @@

#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(const Points::PointKernel& pts)
: myPoints(pts)
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)
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());
Expand All @@ -57,14 +63,67 @@ double SampleConsensus::perform(std::vector<float>& parameters)
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::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
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(inliers);
ransac.getInliers(model);
//ransac.getModel (model);
Eigen::VectorXf model_p_coefficients;
ransac.getModelCoefficients (model_p_coefficients);
Expand Down
17 changes: 15 additions & 2 deletions src/Mod/ReverseEngineering/App/SampleConsensus.h
View file
Expand Up @@ -34,11 +34,24 @@ namespace Reen {
class SampleConsensus
{
public:
SampleConsensus(const Points::PointKernel&);
double perform(std::vector<float>& parameters);
enum SacModel
{
SACMODEL_PLANE,
SACMODEL_LINE,
SACMODEL_CIRCLE2D,
SACMODEL_CIRCLE3D,
SACMODEL_SPHERE,
SACMODEL_CYLINDER,
SACMODEL_CONE,
SACMODEL_TORUS,
};
SampleConsensus(SacModel sac, const Points::PointKernel&, const std::vector<Base::Vector3d>&);
double perform(std::vector<float>& parameters, std::vector<int>& model);

private:
SacModel mySac;
const Points::PointKernel& myPoints;
const std::vector<Base::Vector3d>& myNormals;
};

} // namespace Reen
Expand Down

0 comments on commit 7464c81

Please sign in to comment.