Add test for SAC model normal plane SIMD implementations

test/sample_consensus/test_sample_consensus_plane_models.cpp

@@ -366,6 +366,85 @@ TEST (SampleConsensusModelPlane, SIMD_countWithinDistance) // Test if all countW
   }
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT, typename PointNT>
+class SampleConsensusModelNormalPlaneTest : private SampleConsensusModelNormalPlane<PointT, PointNT>
+{
+  public:
+    using SampleConsensusModelNormalPlane<PointT, PointNT>::SampleConsensusModelNormalPlane;
+    using SampleConsensusModelNormalPlane<PointT, PointNT>::setNormalDistanceWeight;
+    using SampleConsensusModelNormalPlane<PointT, PointNT>::setInputNormals;
+    using SampleConsensusModelNormalPlane<PointT, PointNT>::countWithinDistanceStandard;
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+    using SampleConsensusModelNormalPlane<PointT, PointNT>::countWithinDistanceSSE;
+#endif
+#if defined (__AVX__) && defined (__AVX2__)
+    using SampleConsensusModelNormalPlane<PointT, PointNT>::countWithinDistanceAVX;
+#endif
+};
+
+TEST (SampleConsensusModelNormalPlane, SIMD_countWithinDistance) // Test if all countWithinDistance implementations return the same value
+{
+  const auto seed = static_cast<unsigned> (std::time (nullptr));
+  srand (seed);
+  for (size_t i=0; i<1000; i++) // Run as often as you like
+  {
+    // Generate a cloud with 10000 random points
+    PointCloud<PointXYZ> cloud;
+    PointCloud<Normal> normal_cloud;
+    std::vector<int> indices;
+    cloud.points.resize (10000);
+    normal_cloud.resize (10000);
+    for (std::size_t idx = 0; idx < cloud.size (); ++idx)
+    {
+      cloud.points[idx].x = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      cloud.points[idx].y = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      cloud.points[idx].z = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      const double a = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      const double b = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      const double c = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0;
+      const double factor = 1.0 / sqrt(a * a + b * b + c * c);
+      normal_cloud.points[idx].normal[0] = a * factor;
+      normal_cloud.points[idx].normal[1] = b * factor;
+      normal_cloud.points[idx].normal[2] = c * factor;
+      if (rand () % 4 != 0)
+      {
+        indices.push_back (static_cast<int> (idx));
+      }
+    }
+    SampleConsensusModelNormalPlaneTest<PointXYZ, Normal> model (cloud.makeShared (), indices, true);
+    
+    const double normal_distance_weight = 0.3 * static_cast<double> (rand ()) / RAND_MAX; // in [0; 0.3]
+    model.setNormalDistanceWeight (normal_distance_weight);
+    model.setInputNormals (normal_cloud.makeShared ());
+
+    // Generate random model parameters
+    Eigen::VectorXf model_coefficients(4);
+    model_coefficients << 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
+                          2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0,
+                          2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0, 0.0;
+    model_coefficients.normalize ();
+    model_coefficients(3) = 2.0 * static_cast<float> (rand ()) / RAND_MAX - 1.0; // Last parameter
+
+    const double threshold = 0.1 * static_cast<double> (rand ()) / RAND_MAX; // threshold in [0; 0.1]
+
+    // The number of inliers is usually somewhere between 0 and 100
+    const auto res_standard = model.countWithinDistanceStandard (model_coefficients, threshold); // Standard
+#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
+    const auto res_sse      = model.countWithinDistanceSSE (model_coefficients, threshold); // SSE
+    EXPECT_LE ((res_standard > res_sse ? res_standard - res_sse : res_sse - res_standard), 2u) << "seed=" << seed << ", i=" << i
+        << ", model=(" << model_coefficients(0) << ", " << model_coefficients(1) << ", " << model_coefficients(2) << ", " << model_coefficients(3)
+        << "), threshold=" << threshold << ", normal_distance_weight=" << normal_distance_weight << ", res_standard=" << res_standard << std::endl;
+#endif
+#if defined (__AVX__) && defined (__AVX2__)
+    const auto res_avx      = model.countWithinDistanceAVX (model_coefficients, threshold); // AVX
+    EXPECT_LE ((res_standard > res_avx ? res_standard - res_avx : res_avx - res_standard), 2u) << "seed=" << seed << ", i=" << i
+        << ", model=(" << model_coefficients(0) << ", " << model_coefficients(1) << ", " << model_coefficients(2) << ", " << model_coefficients(3)
+        << "), threshold=" << threshold << ", normal_distance_weight=" << normal_distance_weight << ", res_standard=" << res_standard << std::endl;
+#endif
+  }
+}
+
 int
 main (int argc, char** argv)
 {