FreeCAD

1
/***************************************************************************
2
 *   Copyright (c) 2016 Werner Mayer <wmayer[at]users.sourceforge.net>     *
3
 *                                                                         *
4
 *   This file is part of the FreeCAD CAx development system.              *
5
 *                                                                         *
6
 *   This library is free software; you can redistribute it and/or         *
7
 *   modify it under the terms of the GNU Library General Public           *
8
 *   License as published by the Free Software Foundation; either          *
9
 *   version 2 of the License, or (at your option) any later version.      *
10
 *                                                                         *
11
 *   This library  is distributed in the hope that it will be useful,      *
12
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
13
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
14
 *   GNU Library General Public License for more details.                  *
15
 *                                                                         *
16
 *   You should have received a copy of the GNU Library General Public     *
17
 *   License along with this library; see the file COPYING.LIB. If not,    *
18
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
19
 *   Suite 330, Boston, MA  02111-1307, USA                                *
20
 *                                                                         *
21
 ***************************************************************************/
22

23
#include "PreCompiled.h"
24
#ifndef _PreComp_
25
#include <boost/math/special_functions/fpclassify.hpp>
26
#endif
27

28
#include <Base/Exception.h>
29
#include <Mod/Points/App/Points.h>
30

31
#include "SampleConsensus.h"
32

33

34
#if defined(HAVE_PCL_SAMPLE_CONSENSUS)
35
#include <pcl/features/normal_3d.h>
36
#include <pcl/point_types.h>
37
#include <pcl/sample_consensus/ransac.h>
38
#include <pcl/sample_consensus/sac_model_cone.h>
39
#include <pcl/sample_consensus/sac_model_cylinder.h>
40
#include <pcl/sample_consensus/sac_model_plane.h>
41
#include <pcl/sample_consensus/sac_model_sphere.h>
42

43
using namespace std;
44
using namespace Reen;
45
using pcl::PointCloud;
46
using pcl::PointNormal;
47
using pcl::PointXYZ;
48

49
SampleConsensus::SampleConsensus(SacModel sac,
50
                                 const Points::PointKernel& pts,
51
                                 const std::vector<Base::Vector3d>& nor)
52
    : mySac(sac)
53
    , myPoints(pts)
54
    , myNormals(nor)
55
{}
56

57
double SampleConsensus::perform(std::vector<float>& parameters, std::vector<int>& model)
58
{
59
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
60
    cloud->reserve(myPoints.size());
61
    for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
62
        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y)
63
            && !boost::math::isnan(it->z)) {
64
            cloud->push_back(pcl::PointXYZ(it->x, it->y, it->z));
65
        }
66
    }
67

68
    cloud->width = int(cloud->points.size());
69
    cloud->height = 1;
70
    cloud->is_dense = true;
71

72
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>());
73
    if (mySac == SACMODEL_CONE || mySac == SACMODEL_CYLINDER) {
74
#if 0
75
        // Create search tree
76
        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
77
        tree.reset (new pcl::search::KdTree<PointXYZ> (false));
78
        tree->setInputCloud (cloud);
79

80
        // Normal estimation
81
        int ksearch = 10;
82
        pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
83
        n.setInputCloud (cloud);
84
        n.setSearchMethod (tree);
85
        n.setKSearch (ksearch);
86
        n.compute (*normals);
87
#else
88
        normals->reserve(myNormals.size());
89
        for (std::vector<Base::Vector3d>::const_iterator it = myNormals.begin();
90
             it != myNormals.end();
91
             ++it) {
92
            if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y)
93
                && !boost::math::isnan(it->z)) {
94
                normals->push_back(pcl::Normal(it->x, it->y, it->z));
95
            }
96
        }
97
#endif
98
    }
99

100
    // created RandomSampleConsensus object and compute the appropriated model
101
    pcl::SampleConsensusModel<pcl::PointXYZ>::Ptr model_p;
102
    switch (mySac) {
103
        case SACMODEL_PLANE: {
104
            model_p.reset(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
105
            break;
106
        }
107
        case SACMODEL_SPHERE: {
108
            model_p.reset(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
109
            break;
110
        }
111
        case SACMODEL_CONE: {
112
            pcl::SampleConsensusModelCone<pcl::PointXYZ, pcl::Normal>::Ptr model_c(
113
                new pcl::SampleConsensusModelCone<pcl::PointXYZ, pcl::Normal>(cloud));
114
            model_c->setInputNormals(normals);
115
            model_p = model_c;
116
            break;
117
        }
118
        case SACMODEL_CYLINDER: {
119
            pcl::SampleConsensusModelCylinder<pcl::PointXYZ, pcl::Normal>::Ptr model_c(
120
                new pcl::SampleConsensusModelCylinder<pcl::PointXYZ, pcl::Normal>(cloud));
121
            model_c->setInputNormals(normals);
122
            model_p = model_c;
123
            break;
124
        }
125
        default:
126
            throw Base::RuntimeError("Unsupported SAC model");
127
    }
128

129
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
130
    ransac.setDistanceThreshold(.01);
131
    ransac.computeModel();
132
    ransac.getInliers(model);
133
    // ransac.getModel (model);
134
    Eigen::VectorXf model_p_coefficients;
135
    ransac.getModelCoefficients(model_p_coefficients);
136
    for (int i = 0; i < model_p_coefficients.size(); i++) {
137
        parameters.push_back(model_p_coefficients[i]);
138
    }
139

140
    return ransac.getProbability();
141
}
142

143
#endif  // HAVE_PCL_SAMPLE_CONSENSUS
144