1 /*
2  * Software License Agreement (BSD License)
3  *
4  *  Point Cloud Library (PCL) - www.pointclouds.org
5  *  Copyright (c) 2010-2012, Willow Garage, Inc.
6  *  Copyright (c) 2014-, Open Perception, Inc.
7  *
8  *  All rights reserved.
9  *
10  *  Redistribution and use in source and binary forms, with or without
11  *  modification, are permitted provided that the following conditions
12  *  are met:
13  *
14  *   * Redistributions of source code must retain the above copyright
15  *     notice, this list of conditions and the following disclaimer.
16  *   * Redistributions in binary form must reproduce the above
17  *     copyright notice, this list of conditions and the following
18  *     disclaimer in the documentation and/or other materials provided
19  *     with the distribution.
20  *   * Neither the name of the copyright holder(s) nor the names of its
21  *     contributors may be used to endorse or promote products derived
22  *     from this software without specific prior written permission.
23  *
24  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  *  POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #include <pcl/test/gtest.h>
40 
41 #include <pcl/sample_consensus/ransac.h>
42 #include <pcl/sample_consensus/sac_model_sphere.h>
43 #include <pcl/sample_consensus/sac_model_cone.h>
44 #include <pcl/sample_consensus/sac_model_cylinder.h>
45 #include <pcl/sample_consensus/sac_model_circle.h>
46 #include <pcl/sample_consensus/sac_model_circle3d.h>
47 #include <pcl/sample_consensus/sac_model_normal_sphere.h>
48 
49 using namespace pcl;
50 
51 using SampleConsensusModelSpherePtr = SampleConsensusModelSphere<PointXYZ>::Ptr;
52 using SampleConsensusModelConePtr = SampleConsensusModelCone<PointXYZ, Normal>::Ptr;
53 using SampleConsensusModelCircle2DPtr = SampleConsensusModelCircle2D<PointXYZ>::Ptr;
54 using SampleConsensusModelCircle3DPtr = SampleConsensusModelCircle3D<PointXYZ>::Ptr;
55 using SampleConsensusModelCylinderPtr = SampleConsensusModelCylinder<PointXYZ, Normal>::Ptr;
56 using SampleConsensusModelNormalSpherePtr = SampleConsensusModelNormalSphere<PointXYZ, Normal>::Ptr;
57 
58 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST(SampleConsensusModelSphere,RANSAC)59 TEST (SampleConsensusModelSphere, RANSAC)
60 {
61   srand (0);
62 
63   // Use a custom point cloud for these tests until we need something better
64   PointCloud<PointXYZ> cloud;
65   cloud.resize (10);
66   cloud[0].getVector3fMap () << 1.7068f, 1.0684f, 2.2147f;
67   cloud[1].getVector3fMap () << 2.4708f, 2.3081f, 1.1736f;
68   cloud[2].getVector3fMap () << 2.7609f, 1.9095f, 1.3574f;
69   cloud[3].getVector3fMap () << 2.8016f, 1.6704f, 1.5009f;
70   cloud[4].getVector3fMap () << 1.8517f, 2.0276f, 1.0112f;
71   cloud[5].getVector3fMap () << 1.8726f, 1.3539f, 2.7523f;
72   cloud[6].getVector3fMap () << 2.5179f, 2.3218f, 1.2074f;
73   cloud[7].getVector3fMap () << 2.4026f, 2.5114f, 2.7588f;
74   cloud[8].getVector3fMap () << 2.6999f, 2.5606f, 1.5571f;
75   cloud[9].getVector3fMap () << 0.0000f, 0.0000f, 0.0000f;
76 
77   // Create a shared sphere model pointer directly
78   SampleConsensusModelSpherePtr model (new SampleConsensusModelSphere<PointXYZ> (cloud.makeShared ()));
79 
80   // Create the RANSAC object
81   RandomSampleConsensus<PointXYZ> sac (model, 0.03);
82 
83   // Algorithm tests
84   bool result = sac.computeModel ();
85   ASSERT_TRUE (result);
86 
87   pcl::Indices sample;
88   sac.getModel (sample);
89   EXPECT_EQ (4, sample.size ());
90 
91   pcl::Indices inliers;
92   sac.getInliers (inliers);
93   EXPECT_EQ (9, inliers.size ());
94 
95   Eigen::VectorXf coeff;
96   sac.getModelCoefficients (coeff);
97   EXPECT_EQ (4, coeff.size ());
98   EXPECT_NEAR (2, coeff[0] / coeff[3], 1e-2);
99   EXPECT_NEAR (2, coeff[1] / coeff[3], 1e-2);
100   EXPECT_NEAR (2, coeff[2] / coeff[3], 1e-2);
101 
102   Eigen::VectorXf coeff_refined;
103   model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
104   EXPECT_EQ (4, coeff_refined.size ());
105   EXPECT_NEAR (2, coeff_refined[0] / coeff_refined[3], 1e-2);
106   EXPECT_NEAR (2, coeff_refined[1] / coeff_refined[3], 1e-2);
107   EXPECT_NEAR (2, coeff_refined[2] / coeff_refined[3], 1e-2);
108 }
109 
110 //////////////////////////////////////////////////////////////////////////////////////////////////
111 template <typename PointT>
112 class SampleConsensusModelSphereTest : private SampleConsensusModelSphere<PointT>
113 {
114   public:
115     using SampleConsensusModelSphere<PointT>::SampleConsensusModelSphere;
116     using SampleConsensusModelSphere<PointT>::countWithinDistanceStandard;
117 #if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
118     using SampleConsensusModelSphere<PointT>::countWithinDistanceSSE;
119 #endif
120 #if defined (__AVX__) && defined (__AVX2__)
121     using SampleConsensusModelSphere<PointT>::countWithinDistanceAVX;
122 #endif
123 };
124 
TEST(SampleConsensusModelSphere,SIMD_countWithinDistance)125 TEST (SampleConsensusModelSphere, SIMD_countWithinDistance) // Test if all countWithinDistance implementations return the same value
126 {
127   const auto seed = static_cast<unsigned> (std::time (nullptr));
128   srand (seed);
129   for (size_t i=0; i<100; i++) // Run as often as you like
130   {
131     // Generate a cloud with 1000 random points
132     PointCloud<PointXYZ> cloud;
133     pcl::Indices indices;
134     cloud.resize (1000);
135     for (std::size_t idx = 0; idx < cloud.size (); ++idx)
136     {
137       cloud[idx].x = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
138       cloud[idx].y = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
139       cloud[idx].z = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
140       if (rand () % 3 != 0)
141       {
142         indices.push_back (static_cast<int> (idx));
143       }
144     }
145     SampleConsensusModelSphereTest<PointXYZ> model (cloud.makeShared (), indices, true);
146 
147     // Generate random sphere model parameters
148     Eigen::VectorXf model_coefficients(4);
149     model_coefficients << 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
150                           2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
151                           2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
152                           0.15 * static_cast<float> (rand ()) / RAND_MAX; // center and radius
153 
154     const double threshold = 0.15 * static_cast<double> (rand ()) / RAND_MAX; // threshold in [0; 0.1]
155 
156     // The number of inliers is usually somewhere between 0 and 10
157     const auto res_standard = model.countWithinDistanceStandard (model_coefficients, threshold); // Standard
158     PCL_DEBUG ("seed=%lu, i=%lu, model=(%f, %f, %f, %f), threshold=%f, res_standard=%lu\n", seed, i,
159                model_coefficients(0), model_coefficients(1), model_coefficients(2), model_coefficients(3), threshold, res_standard);
160 #if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
161     const auto res_sse      = model.countWithinDistanceSSE (model_coefficients, threshold); // SSE
162     ASSERT_EQ (res_standard, res_sse);
163 #endif
164 #if defined (__AVX__) && defined (__AVX2__)
165     const auto res_avx      = model.countWithinDistanceAVX (model_coefficients, threshold); // AVX
166     ASSERT_EQ (res_standard, res_avx);
167 #endif
168   }
169 }
170 
171 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST(SampleConsensusModelNormalSphere,RANSAC)172 TEST (SampleConsensusModelNormalSphere, RANSAC)
173 {
174   srand (0);
175 
176   // Use a custom point cloud for these tests until we need something better
177   PointCloud<PointXYZ> cloud;
178   PointCloud<Normal> normals;
179   cloud.resize (27); normals.resize (27);
180   cloud[ 0].getVector3fMap () << -0.014695f,  0.009549f, 0.954775f;
181   cloud[ 1].getVector3fMap () <<  0.014695f,  0.009549f, 0.954775f;
182   cloud[ 2].getVector3fMap () << -0.014695f,  0.040451f, 0.954775f;
183   cloud[ 3].getVector3fMap () <<  0.014695f,  0.040451f, 0.954775f;
184   cloud[ 4].getVector3fMap () << -0.009082f, -0.015451f, 0.972049f;
185   cloud[ 5].getVector3fMap () <<  0.009082f, -0.015451f, 0.972049f;
186   cloud[ 6].getVector3fMap () << -0.038471f,  0.009549f, 0.972049f;
187   cloud[ 7].getVector3fMap () <<  0.038471f,  0.009549f, 0.972049f;
188   cloud[ 8].getVector3fMap () << -0.038471f,  0.040451f, 0.972049f;
189   cloud[ 9].getVector3fMap () <<  0.038471f,  0.040451f, 0.972049f;
190   cloud[10].getVector3fMap () << -0.009082f,  0.065451f, 0.972049f;
191   cloud[11].getVector3fMap () <<  0.009082f,  0.065451f, 0.972049f;
192   cloud[12].getVector3fMap () << -0.023776f, -0.015451f, 0.982725f;
193   cloud[13].getVector3fMap () <<  0.023776f, -0.015451f, 0.982725f;
194   cloud[14].getVector3fMap () << -0.023776f,  0.065451f, 0.982725f;
195   cloud[15].getVector3fMap () <<  0.023776f,  0.065451f, 0.982725f;
196   cloud[16].getVector3fMap () << -0.000000f, -0.025000f, 1.000000f;
197   cloud[17].getVector3fMap () <<  0.000000f, -0.025000f, 1.000000f;
198   cloud[18].getVector3fMap () << -0.029389f, -0.015451f, 1.000000f;
199   cloud[19].getVector3fMap () <<  0.029389f, -0.015451f, 1.000000f;
200   cloud[20].getVector3fMap () << -0.047553f,  0.009549f, 1.000000f;
201   cloud[21].getVector3fMap () <<  0.047553f,  0.009549f, 1.000000f;
202   cloud[22].getVector3fMap () << -0.047553f,  0.040451f, 1.000000f;
203   cloud[23].getVector3fMap () <<  0.047553f,  0.040451f, 1.000000f;
204   cloud[24].getVector3fMap () << -0.029389f,  0.065451f, 1.000000f;
205   cloud[25].getVector3fMap () <<  0.029389f,  0.065451f, 1.000000f;
206   cloud[26].getVector3fMap () <<  0.000000f,  0.075000f, 1.000000f;
207 
208   normals[ 0].getNormalVector3fMap () << -0.293893f, -0.309017f, -0.904509f;
209   normals[ 1].getNormalVector3fMap () <<  0.293893f, -0.309017f, -0.904508f;
210   normals[ 2].getNormalVector3fMap () << -0.293893f,  0.309017f, -0.904509f;
211   normals[ 3].getNormalVector3fMap () <<  0.293893f,  0.309017f, -0.904508f;
212   normals[ 4].getNormalVector3fMap () << -0.181636f, -0.809017f, -0.559017f;
213   normals[ 5].getNormalVector3fMap () <<  0.181636f, -0.809017f, -0.559017f;
214   normals[ 6].getNormalVector3fMap () << -0.769421f, -0.309017f, -0.559017f;
215   normals[ 7].getNormalVector3fMap () <<  0.769421f, -0.309017f, -0.559017f;
216   normals[ 8].getNormalVector3fMap () << -0.769421f,  0.309017f, -0.559017f;
217   normals[ 9].getNormalVector3fMap () <<  0.769421f,  0.309017f, -0.559017f;
218   normals[10].getNormalVector3fMap () << -0.181636f,  0.809017f, -0.559017f;
219   normals[11].getNormalVector3fMap () <<  0.181636f,  0.809017f, -0.559017f;
220   normals[12].getNormalVector3fMap () << -0.475528f, -0.809017f, -0.345491f;
221   normals[13].getNormalVector3fMap () <<  0.475528f, -0.809017f, -0.345491f;
222   normals[14].getNormalVector3fMap () << -0.475528f,  0.809017f, -0.345491f;
223   normals[15].getNormalVector3fMap () <<  0.475528f,  0.809017f, -0.345491f;
224   normals[16].getNormalVector3fMap () << -0.000000f, -1.000000f,  0.000000f;
225   normals[17].getNormalVector3fMap () <<  0.000000f, -1.000000f,  0.000000f;
226   normals[18].getNormalVector3fMap () << -0.587785f, -0.809017f,  0.000000f;
227   normals[19].getNormalVector3fMap () <<  0.587785f, -0.809017f,  0.000000f;
228   normals[20].getNormalVector3fMap () << -0.951057f, -0.309017f,  0.000000f;
229   normals[21].getNormalVector3fMap () <<  0.951057f, -0.309017f,  0.000000f;
230   normals[22].getNormalVector3fMap () << -0.951057f,  0.309017f,  0.000000f;
231   normals[23].getNormalVector3fMap () <<  0.951057f,  0.309017f,  0.000000f;
232   normals[24].getNormalVector3fMap () << -0.587785f,  0.809017f,  0.000000f;
233   normals[25].getNormalVector3fMap () <<  0.587785f,  0.809017f,  0.000000f;
234   normals[26].getNormalVector3fMap () <<  0.000000f,  1.000000f,  0.000000f;
235 
236   // Create a shared sphere model pointer directly
237   SampleConsensusModelNormalSpherePtr model (new SampleConsensusModelNormalSphere<PointXYZ, Normal> (cloud.makeShared ()));
238   model->setInputNormals (normals.makeShared ());
239 
240   // Create the RANSAC object
241   RandomSampleConsensus<PointXYZ> sac (model, 0.03);
242 
243   // Algorithm tests
244   bool result = sac.computeModel ();
245   ASSERT_TRUE (result);
246 
247   pcl::Indices sample;
248   sac.getModel (sample);
249   EXPECT_EQ (4, sample.size ());
250 
251   pcl::Indices inliers;
252   sac.getInliers (inliers);
253   EXPECT_EQ (27, inliers.size ());
254 
255   Eigen::VectorXf coeff;
256   sac.getModelCoefficients (coeff);
257   EXPECT_EQ (4, coeff.size ());
258   EXPECT_NEAR (0.000, coeff[0], 1e-2);
259   EXPECT_NEAR (0.025, coeff[1], 1e-2);
260   EXPECT_NEAR (1.000, coeff[2], 1e-2);
261   EXPECT_NEAR (0.050, coeff[3], 1e-2);
262 
263   Eigen::VectorXf coeff_refined;
264   model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
265   EXPECT_EQ (4, coeff_refined.size ());
266   EXPECT_NEAR (0.000, coeff_refined[0], 1e-2);
267   EXPECT_NEAR (0.025, coeff_refined[1], 1e-2);
268   EXPECT_NEAR (1.000, coeff_refined[2], 1e-2);
269   EXPECT_NEAR (0.050, coeff_refined[3], 1e-2);
270 }
271 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST(SampleConsensusModelCone,RANSAC)272 TEST (SampleConsensusModelCone, RANSAC)
273 {
274   srand (0);
275 
276   // Use a custom point cloud for these tests until we need something better
277   PointCloud<PointXYZ> cloud;
278   PointCloud<Normal> normals;
279   cloud.resize (31); normals.resize (31);
280 
281   cloud[ 0].getVector3fMap () << -0.011247f, 0.200000f, 0.965384f;
282   cloud[ 1].getVector3fMap () <<  0.000000f, 0.200000f, 0.963603f;
283   cloud[ 2].getVector3fMap () <<  0.011247f, 0.200000f, 0.965384f;
284   cloud[ 3].getVector3fMap () << -0.016045f, 0.175000f, 0.977916f;
285   cloud[ 4].getVector3fMap () << -0.008435f, 0.175000f, 0.974038f;
286   cloud[ 5].getVector3fMap () <<  0.004218f, 0.175000f, 0.973370f;
287   cloud[ 6].getVector3fMap () <<  0.016045f, 0.175000f, 0.977916f;
288   cloud[ 7].getVector3fMap () << -0.025420f, 0.200000f, 0.974580f;
289   cloud[ 8].getVector3fMap () <<  0.025420f, 0.200000f, 0.974580f;
290   cloud[ 9].getVector3fMap () << -0.012710f, 0.150000f, 0.987290f;
291   cloud[10].getVector3fMap () << -0.005624f, 0.150000f, 0.982692f;
292   cloud[11].getVector3fMap () <<  0.002812f, 0.150000f, 0.982247f;
293   cloud[12].getVector3fMap () <<  0.012710f, 0.150000f, 0.987290f;
294   cloud[13].getVector3fMap () << -0.022084f, 0.175000f, 0.983955f;
295   cloud[14].getVector3fMap () <<  0.022084f, 0.175000f, 0.983955f;
296   cloud[15].getVector3fMap () << -0.034616f, 0.200000f, 0.988753f;
297   cloud[16].getVector3fMap () <<  0.034616f, 0.200000f, 0.988753f;
298   cloud[17].getVector3fMap () << -0.006044f, 0.125000f, 0.993956f;
299   cloud[18].getVector3fMap () <<  0.004835f, 0.125000f, 0.993345f;
300   cloud[19].getVector3fMap () << -0.017308f, 0.150000f, 0.994376f;
301   cloud[20].getVector3fMap () <<  0.017308f, 0.150000f, 0.994376f;
302   cloud[21].getVector3fMap () << -0.025962f, 0.175000f, 0.991565f;
303   cloud[22].getVector3fMap () <<  0.025962f, 0.175000f, 0.991565f;
304   cloud[23].getVector3fMap () << -0.009099f, 0.125000f, 1.000000f;
305   cloud[24].getVector3fMap () <<  0.009099f, 0.125000f, 1.000000f;
306   cloud[25].getVector3fMap () << -0.018199f, 0.150000f, 1.000000f;
307   cloud[26].getVector3fMap () <<  0.018199f, 0.150000f, 1.000000f;
308   cloud[27].getVector3fMap () << -0.027298f, 0.175000f, 1.000000f;
309   cloud[28].getVector3fMap () <<  0.027298f, 0.175000f, 1.000000f;
310   cloud[29].getVector3fMap () << -0.036397f, 0.200000f, 1.000000f;
311   cloud[30].getVector3fMap () <<  0.036397f, 0.200000f, 1.000000f;
312 
313   normals[ 0].getNormalVector3fMap () << -0.290381f, -0.342020f, -0.893701f;
314   normals[ 1].getNormalVector3fMap () <<  0.000000f, -0.342020f, -0.939693f;
315   normals[ 2].getNormalVector3fMap () <<  0.290381f, -0.342020f, -0.893701f;
316   normals[ 3].getNormalVector3fMap () << -0.552338f, -0.342020f, -0.760227f;
317   normals[ 4].getNormalVector3fMap () << -0.290381f, -0.342020f, -0.893701f;
318   normals[ 5].getNormalVector3fMap () <<  0.145191f, -0.342020f, -0.916697f;
319   normals[ 6].getNormalVector3fMap () <<  0.552337f, -0.342020f, -0.760227f;
320   normals[ 7].getNormalVector3fMap () << -0.656282f, -0.342020f, -0.656283f;
321   normals[ 8].getNormalVector3fMap () <<  0.656282f, -0.342020f, -0.656283f;
322   normals[ 9].getNormalVector3fMap () << -0.656283f, -0.342020f, -0.656282f;
323   normals[10].getNormalVector3fMap () << -0.290381f, -0.342020f, -0.893701f;
324   normals[11].getNormalVector3fMap () <<  0.145191f, -0.342020f, -0.916697f;
325   normals[12].getNormalVector3fMap () <<  0.656282f, -0.342020f, -0.656282f;
326   normals[13].getNormalVector3fMap () << -0.760228f, -0.342020f, -0.552337f;
327   normals[14].getNormalVector3fMap () <<  0.760228f, -0.342020f, -0.552337f;
328   normals[15].getNormalVector3fMap () << -0.893701f, -0.342020f, -0.290380f;
329   normals[16].getNormalVector3fMap () <<  0.893701f, -0.342020f, -0.290380f;
330   normals[17].getNormalVector3fMap () << -0.624162f, -0.342020f, -0.624162f;
331   normals[18].getNormalVector3fMap () <<  0.499329f, -0.342020f, -0.687268f;
332   normals[19].getNormalVector3fMap () << -0.893701f, -0.342020f, -0.290380f;
333   normals[20].getNormalVector3fMap () <<  0.893701f, -0.342020f, -0.290380f;
334   normals[21].getNormalVector3fMap () << -0.893701f, -0.342020f, -0.290381f;
335   normals[22].getNormalVector3fMap () <<  0.893701f, -0.342020f, -0.290381f;
336   normals[23].getNormalVector3fMap () << -0.939693f, -0.342020f,  0.000000f;
337   normals[24].getNormalVector3fMap () <<  0.939693f, -0.342020f,  0.000000f;
338   normals[25].getNormalVector3fMap () << -0.939693f, -0.342020f,  0.000000f;
339   normals[26].getNormalVector3fMap () <<  0.939693f, -0.342020f,  0.000000f;
340   normals[27].getNormalVector3fMap () << -0.939693f, -0.342020f,  0.000000f;
341   normals[28].getNormalVector3fMap () <<  0.939693f, -0.342020f,  0.000000f;
342   normals[29].getNormalVector3fMap () << -0.939693f, -0.342020f,  0.000000f;
343   normals[30].getNormalVector3fMap () <<  0.939693f, -0.342020f,  0.000000f;
344 
345   // Create a shared cylinder model pointer directly
346   SampleConsensusModelConePtr model (new SampleConsensusModelCone<PointXYZ, Normal> (cloud.makeShared ()));
347   model->setInputNormals (normals.makeShared ());
348 
349   // Create the RANSAC object
350   RandomSampleConsensus<PointXYZ> sac (model, 0.03);
351 
352   // Algorithm tests
353   bool result = sac.computeModel ();
354   ASSERT_TRUE (result);
355 
356   pcl::Indices sample;
357   sac.getModel (sample);
358   EXPECT_EQ (3, sample.size ());
359 
360   pcl::Indices inliers;
361   sac.getInliers (inliers);
362   EXPECT_EQ (31, inliers.size ());
363 
364   Eigen::VectorXf coeff;
365   sac.getModelCoefficients (coeff);
366   EXPECT_EQ (7, coeff.size ());
367   EXPECT_NEAR (0.000000, coeff[0], 1e-2);
368   EXPECT_NEAR (0.100000, coeff[1], 1e-2);
369   EXPECT_NEAR (0.349066, coeff[6], 1e-2);
370 
371   Eigen::VectorXf coeff_refined;
372   model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
373   EXPECT_EQ (7, coeff_refined.size ());
374   EXPECT_NEAR (0.349066, coeff_refined[6], 1e-2);
375 }
376 
377 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST(SampleConsensusModelCylinder,RANSAC)378 TEST (SampleConsensusModelCylinder, RANSAC)
379 {
380   srand (0);
381 
382   // Use a custom point cloud for these tests until we need something better
383   PointCloud<PointXYZ> cloud;
384   PointCloud<Normal> normals;
385   cloud.resize (20); normals.resize (20);
386 
387   cloud[ 0].getVector3fMap () << -0.499902f, 2.199701f, 0.000008f;
388   cloud[ 1].getVector3fMap () << -0.875397f, 2.030177f, 0.050104f;
389   cloud[ 2].getVector3fMap () << -0.995875f, 1.635973f, 0.099846f;
390   cloud[ 3].getVector3fMap () << -0.779523f, 1.285527f, 0.149961f;
391   cloud[ 4].getVector3fMap () << -0.373285f, 1.216488f, 0.199959f;
392   cloud[ 5].getVector3fMap () << -0.052893f, 1.475973f, 0.250101f;
393   cloud[ 6].getVector3fMap () << -0.036558f, 1.887591f, 0.299839f;
394   cloud[ 7].getVector3fMap () << -0.335048f, 2.171994f, 0.350001f;
395   cloud[ 8].getVector3fMap () << -0.745456f, 2.135528f, 0.400072f;
396   cloud[ 9].getVector3fMap () << -0.989282f, 1.803311f, 0.449983f;
397   cloud[10].getVector3fMap () << -0.900651f, 1.400701f, 0.500126f;
398   cloud[11].getVector3fMap () << -0.539658f, 1.201468f, 0.550079f;
399   cloud[12].getVector3fMap () << -0.151875f, 1.340951f, 0.599983f;
400   cloud[13].getVector3fMap () << -0.000724f, 1.724373f, 0.649882f;
401   cloud[14].getVector3fMap () << -0.188573f, 2.090983f, 0.699854f;
402   cloud[15].getVector3fMap () << -0.587925f, 2.192257f, 0.749956f;
403   cloud[16].getVector3fMap () << -0.927724f, 1.958846f, 0.800008f;
404   cloud[17].getVector3fMap () << -0.976888f, 1.549655f, 0.849970f;
405   cloud[18].getVector3fMap () << -0.702003f, 1.242707f, 0.899954f;
406   cloud[19].getVector3fMap () << -0.289916f, 1.246296f, 0.950075f;
407 
408   normals[ 0].getNormalVector3fMap () <<  0.000098f,  1.000098f,  0.000008f;
409   normals[ 1].getNormalVector3fMap () << -0.750891f,  0.660413f,  0.000104f;
410   normals[ 2].getNormalVector3fMap () << -0.991765f, -0.127949f, -0.000154f;
411   normals[ 3].getNormalVector3fMap () << -0.558918f, -0.829439f, -0.000039f;
412   normals[ 4].getNormalVector3fMap () <<  0.253627f, -0.967447f, -0.000041f;
413   normals[ 5].getNormalVector3fMap () <<  0.894105f, -0.447965f,  0.000101f;
414   normals[ 6].getNormalVector3fMap () <<  0.926852f,  0.375543f, -0.000161f;
415   normals[ 7].getNormalVector3fMap () <<  0.329948f,  0.943941f,  0.000001f;
416   normals[ 8].getNormalVector3fMap () << -0.490966f,  0.871203f,  0.000072f;
417   normals[ 9].getNormalVector3fMap () << -0.978507f,  0.206425f, -0.000017f;
418   normals[10].getNormalVector3fMap () << -0.801227f, -0.598534f,  0.000126f;
419   normals[11].getNormalVector3fMap () << -0.079447f, -0.996697f,  0.000079f;
420   normals[12].getNormalVector3fMap () <<  0.696154f, -0.717889f, -0.000017f;
421   normals[13].getNormalVector3fMap () <<  0.998685f,  0.048502f, -0.000118f;
422   normals[14].getNormalVector3fMap () <<  0.622933f,  0.782133f, -0.000146f;
423   normals[15].getNormalVector3fMap () << -0.175948f,  0.984480f, -0.000044f;
424   normals[16].getNormalVector3fMap () << -0.855476f,  0.517824f,  0.000008f;
425   normals[17].getNormalVector3fMap () << -0.953769f, -0.300571f, -0.000030f;
426   normals[18].getNormalVector3fMap () << -0.404035f, -0.914700f, -0.000046f;
427   normals[19].getNormalVector3fMap () <<  0.420154f, -0.907445f,  0.000075f;
428 
429   // Create a shared cylinder model pointer directly
430   SampleConsensusModelCylinderPtr model (new SampleConsensusModelCylinder<PointXYZ, Normal> (cloud.makeShared ()));
431   model->setInputNormals (normals.makeShared ());
432 
433   // Create the RANSAC object
434   RandomSampleConsensus<PointXYZ> sac (model, 0.03);
435 
436   // Algorithm tests
437   bool result = sac.computeModel ();
438   ASSERT_TRUE (result);
439 
440   pcl::Indices sample;
441   sac.getModel (sample);
442   EXPECT_EQ (2, sample.size ());
443 
444   pcl::Indices inliers;
445   sac.getInliers (inliers);
446   EXPECT_EQ (20, inliers.size ());
447 
448   Eigen::VectorXf coeff;
449   sac.getModelCoefficients (coeff);
450   EXPECT_EQ (7, coeff.size ());
451   EXPECT_NEAR (-0.5, coeff[0], 1e-3);
452   EXPECT_NEAR ( 1.7, coeff[1], 1e-3);
453   EXPECT_NEAR ( 0.5, coeff[6], 1e-3);
454 
455   Eigen::VectorXf coeff_refined;
456   model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
457   EXPECT_EQ (7, coeff_refined.size ());
458   EXPECT_NEAR (0.5, coeff_refined[6], 1e-3);
459 }
460 
461 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST(SampleConsensusModelCircle2D,RANSAC)462 TEST (SampleConsensusModelCircle2D, RANSAC)
463 {
464   srand (0);
465 
466   // Use a custom point cloud for these tests until we need something better
467   PointCloud<PointXYZ> cloud;
468   cloud.resize (18);
469 
470   cloud[ 0].getVector3fMap () << 3.587751f, -4.190982f, 0.0f;
471   cloud[ 1].getVector3fMap () << 3.808883f, -4.412265f, 0.0f;
472   cloud[ 2].getVector3fMap () << 3.587525f, -5.809143f, 0.0f;
473   cloud[ 3].getVector3fMap () << 2.999913f, -5.999980f, 0.0f;
474   cloud[ 4].getVector3fMap () << 2.412224f, -5.809090f, 0.0f;
475   cloud[ 5].getVector3fMap () << 2.191080f, -5.587682f, 0.0f;
476   cloud[ 6].getVector3fMap () << 2.048941f, -5.309003f, 0.0f;
477   cloud[ 7].getVector3fMap () << 2.000397f, -4.999944f, 0.0f;
478   cloud[ 8].getVector3fMap () << 2.999953f, -6.000056f, 0.0f;
479   cloud[ 9].getVector3fMap () << 2.691127f, -5.951136f, 0.0f;
480   cloud[10].getVector3fMap () << 2.190892f, -5.587838f, 0.0f;
481   cloud[11].getVector3fMap () << 2.048874f, -5.309052f, 0.0f;
482   cloud[12].getVector3fMap () << 1.999990f, -5.000147f, 0.0f;
483   cloud[13].getVector3fMap () << 2.049026f, -4.690918f, 0.0f;
484   cloud[14].getVector3fMap () << 2.190956f, -4.412162f, 0.0f;
485   cloud[15].getVector3fMap () << 2.412231f, -4.190918f, 0.0f;
486   cloud[16].getVector3fMap () << 2.691027f, -4.049060f, 0.0f;
487   cloud[17].getVector3fMap () << 2.000000f, -3.000000f, 0.0f;
488 
489   // Create a shared 2d circle model pointer directly
490   SampleConsensusModelCircle2DPtr model (new SampleConsensusModelCircle2D<PointXYZ> (cloud.makeShared ()));
491 
492   // Create the RANSAC object
493   RandomSampleConsensus<PointXYZ> sac (model, 0.03);
494 
495   // Algorithm tests
496   bool result = sac.computeModel ();
497   ASSERT_TRUE (result);
498 
499   pcl::Indices sample;
500   sac.getModel (sample);
501   EXPECT_EQ (3, sample.size ());
502 
503   pcl::Indices inliers;
504   sac.getInliers (inliers);
505   EXPECT_EQ (17, inliers.size ());
506 
507   Eigen::VectorXf coeff;
508   sac.getModelCoefficients (coeff);
509   EXPECT_EQ (3, coeff.size ());
510   EXPECT_NEAR ( 3, coeff[0], 1e-3);
511   EXPECT_NEAR (-5, coeff[1], 1e-3);
512   EXPECT_NEAR ( 1, coeff[2], 1e-3);
513 
514   Eigen::VectorXf coeff_refined;
515   model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
516   EXPECT_EQ (3, coeff_refined.size ());
517   EXPECT_NEAR ( 3, coeff_refined[0], 1e-3);
518   EXPECT_NEAR (-5, coeff_refined[1], 1e-3);
519   EXPECT_NEAR ( 1, coeff_refined[2], 1e-3);
520 }
521 
522 //////////////////////////////////////////////////////////////////////////////////////////////////
523 template <typename PointT>
524 class SampleConsensusModelCircle2DTest : private SampleConsensusModelCircle2D<PointT>
525 {
526   public:
527     using SampleConsensusModelCircle2D<PointT>::SampleConsensusModelCircle2D;
528     using SampleConsensusModelCircle2D<PointT>::countWithinDistanceStandard;
529 #if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
530     using SampleConsensusModelCircle2D<PointT>::countWithinDistanceSSE;
531 #endif
532 #if defined (__AVX__) && defined (__AVX2__)
533     using SampleConsensusModelCircle2D<PointT>::countWithinDistanceAVX;
534 #endif
535 };
536 
TEST(SampleConsensusModelCircle2D,SIMD_countWithinDistance)537 TEST (SampleConsensusModelCircle2D, SIMD_countWithinDistance) // Test if all countWithinDistance implementations return the same value
538 {
539   const auto seed = static_cast<unsigned> (std::time (nullptr));
540   srand (seed);
541   for (size_t i=0; i<100; i++) // Run as often as you like
542   {
543     // Generate a cloud with 1000 random points
544     PointCloud<PointXYZ> cloud;
545     pcl::Indices indices;
546     cloud.resize (1000);
547     for (std::size_t idx = 0; idx < cloud.size (); ++idx)
548     {
549       cloud[idx].x = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
550       cloud[idx].y = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
551       cloud[idx].z = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
552       if (rand () % 2 == 0)
553       {
554         indices.push_back (static_cast<int> (idx));
555       }
556     }
557     SampleConsensusModelCircle2DTest<PointXYZ> model (cloud.makeShared (), indices, true);
558 
559     // Generate random circle model parameters
560     Eigen::VectorXf model_coefficients(3);
561     model_coefficients << 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
562                           2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
563                           0.1 * static_cast<float> (rand ()) / RAND_MAX; // center and radius
564 
565     const double threshold = 0.1 * static_cast<double> (rand ()) / RAND_MAX; // threshold in [0; 0.1]
566 
567     // The number of inliers is usually somewhere between 0 and 20
568     const auto res_standard = model.countWithinDistanceStandard (model_coefficients, threshold); // Standard
569     PCL_DEBUG ("seed=%lu, i=%lu, model=(%f, %f, %f), threshold=%f, res_standard=%lu\n", seed, i,
570                model_coefficients(0), model_coefficients(1), model_coefficients(2), threshold, res_standard);
571 #if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
572     const auto res_sse      = model.countWithinDistanceSSE (model_coefficients, threshold); // SSE
573     ASSERT_EQ (res_standard, res_sse);
574 #endif
575 #if defined (__AVX__) && defined (__AVX2__)
576     const auto res_avx      = model.countWithinDistanceAVX (model_coefficients, threshold); // AVX
577     ASSERT_EQ (res_standard, res_avx);
578 #endif
579   }
580 }
581 
582 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST(SampleConsensusModelCircle3D,RANSAC)583 TEST (SampleConsensusModelCircle3D, RANSAC)
584 {
585   srand (0);
586 
587   // Use a custom point cloud for these tests until we need something better
588   PointCloud<PointXYZ> cloud;
589   cloud.resize (20);
590 
591   cloud[ 0].getVector3fMap () << 1.00000000f, 5.0000000f, -2.9000001f;
592   cloud[ 1].getVector3fMap () << 1.03420200f, 5.0000000f, -2.9060307f;
593   cloud[ 2].getVector3fMap () << 1.06427870f, 5.0000000f, -2.9233956f;
594   cloud[ 3].getVector3fMap () << 1.08660260f, 5.0000000f, -2.9500000f;
595   cloud[ 4].getVector3fMap () << 1.09848080f, 5.0000000f, -2.9826353f;
596   cloud[ 5].getVector3fMap () << 1.09848080f, 5.0000000f, -3.0173647f;
597   cloud[ 6].getVector3fMap () << 1.08660260f, 5.0000000f, -3.0500000f;
598   cloud[ 7].getVector3fMap () << 1.06427870f, 5.0000000f, -3.0766044f;
599   cloud[ 8].getVector3fMap () << 1.03420200f, 5.0000000f, -3.0939693f;
600   cloud[ 9].getVector3fMap () << 1.00000000f, 5.0000000f, -3.0999999f;
601   cloud[10].getVector3fMap () << 0.96579796f, 5.0000000f, -3.0939693f;
602   cloud[11].getVector3fMap () << 0.93572122f, 5.0000000f, -3.0766044f;
603   cloud[12].getVector3fMap () << 0.91339743f, 5.0000000f, -3.0500000f;
604   cloud[13].getVector3fMap () << 0.90151924f, 5.0000000f, -3.0173647f;
605   cloud[14].getVector3fMap () << 0.90151924f, 5.0000000f, -2.9826353f;
606   cloud[15].getVector3fMap () << 0.91339743f, 5.0000000f, -2.9500000f;
607   cloud[16].getVector3fMap () << 0.93572122f, 5.0000000f, -2.9233956f;
608   cloud[17].getVector3fMap () << 0.96579796f, 5.0000000f, -2.9060307f;
609   cloud[18].getVector3fMap () << 0.85000002f, 4.8499999f, -3.1500001f;
610   cloud[19].getVector3fMap () << 1.15000000f, 5.1500001f, -2.8499999f;
611 
612   // Create a shared 3d circle model pointer directly
613   SampleConsensusModelCircle3DPtr model (new SampleConsensusModelCircle3D<PointXYZ> (cloud.makeShared ()));
614 
615   // Create the RANSAC object
616   RandomSampleConsensus<PointXYZ> sac (model, 0.03);
617 
618   // Algorithm tests
619   bool result = sac.computeModel ();
620   ASSERT_TRUE (result);
621 
622   pcl::Indices sample;
623   sac.getModel (sample);
624   EXPECT_EQ (3, sample.size ());
625 
626   pcl::Indices inliers;
627   sac.getInliers (inliers);
628   EXPECT_EQ (18, inliers.size ());
629 
630   Eigen::VectorXf coeff;
631   sac.getModelCoefficients (coeff);
632   EXPECT_EQ (7, coeff.size ());
633   EXPECT_NEAR ( 1.0, coeff[0], 1e-3);
634   EXPECT_NEAR ( 5.0, coeff[1], 1e-3);
635   EXPECT_NEAR (-3.0, coeff[2], 1e-3);
636   EXPECT_NEAR ( 0.1, coeff[3], 1e-3);
637   EXPECT_NEAR ( 0.0, coeff[4], 1e-3);
638   EXPECT_NEAR (-1.0, coeff[5], 1e-3);
639   EXPECT_NEAR ( 0.0, coeff[6], 1e-3);
640 
641   Eigen::VectorXf coeff_refined;
642   model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
643   EXPECT_EQ (7, coeff_refined.size ());
644   EXPECT_NEAR ( 1.0, coeff_refined[0], 1e-3);
645   EXPECT_NEAR ( 5.0, coeff_refined[1], 1e-3);
646   EXPECT_NEAR (-3.0, coeff_refined[2], 1e-3);
647   EXPECT_NEAR ( 0.1, coeff_refined[3], 1e-3);
648   EXPECT_NEAR ( 0.0, coeff_refined[4], 1e-3);
649   EXPECT_NEAR (-1.0, coeff_refined[5], 1e-3);
650   EXPECT_NEAR ( 0.0, coeff_refined[6], 1e-3);
651 }
652 
653 int
main(int argc,char ** argv)654 main (int argc, char** argv)
655 {
656   testing::InitGoogleTest (&argc, argv);
657   return (RUN_ALL_TESTS ());
658 }
659 
660