Restore original test values and add additional checks to the SAC cone model.

Author: SergioRAgostinho

test/sample_consensus/test_sample_consensus_quadric_models.cpp

@@ -303,13 +303,29 @@ TEST (SampleConsensusModelCone, RANSAC)
   Eigen::VectorXf coeff;
   sac.getModelCoefficients (coeff);
   EXPECT_EQ (7, coeff.size ());
+  // apex
   EXPECT_NEAR (0.000000, coeff[0], 1e-2);
   EXPECT_NEAR (0.100000, coeff[1], 1e-2);
-  EXPECT_NEAR (0.510614, coeff[6], 1e-2);
+  EXPECT_NEAR (1.000000, coeff[2], 1e-2);
+  // axis, defined as a normal to a given plane
+  EXPECT_NEAR (0.000000, std::abs (coeff[3]), 1e-2);
+  EXPECT_NEAR (1.000000, std::abs (coeff[4]), 1e-2);
+  EXPECT_NEAR (0.000000, std::abs (coeff[5]), 1e-2);
+  // cones opening angle
+  EXPECT_NEAR (0.349066, coeff[6], 1e-2);
 
   Eigen::VectorXf coeff_refined;
   model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
   EXPECT_EQ (7, coeff_refined.size ());
+  // apex
+  EXPECT_NEAR (0.000000, coeff_refined[0], 1e-2);
+  EXPECT_NEAR (0.100000, coeff_refined[1], 1e-2);
+  EXPECT_NEAR (1.000000, coeff_refined[2], 1e-2);
+  // axis, defined as a normal to a given plane
+  EXPECT_NEAR (0.000000, std::abs (coeff_refined[3]), 1e-2);
+  EXPECT_NEAR (1.000000, std::abs (coeff_refined[4]), 1e-2);
+  EXPECT_NEAR (0.000000, std::abs (coeff_refined[5]), 1e-2);
+  // cones opening angle
   EXPECT_NEAR (0.349066, coeff_refined[6], 1e-2);
 }
 
@@ -387,9 +403,9 @@ TEST (SampleConsensusModelCylinder, RANSAC)
   Eigen::VectorXf coeff;
   sac.getModelCoefficients (coeff);
   EXPECT_EQ (7, coeff.size ());
-  EXPECT_NEAR (-0.49, coeff[0], 1e-3);
-  EXPECT_NEAR ( 1.69, coeff[1], 1e-3);
-  EXPECT_NEAR ( 0.49, coeff[6], 1e-3);
+  EXPECT_NEAR (-0.5, coeff[0], 1e-3);
+  EXPECT_NEAR ( 1.7, coeff[1], 1e-3);
+  EXPECT_NEAR ( 0.5, coeff[6], 1e-3);
 
   Eigen::VectorXf coeff_refined;
   model->optimizeModelCoefficients (inliers, coeff, coeff_refined);