Merge pull request #1367 from taketwo/sac-refactoring-2-2

SAC Models Refactoring: isModelValid(), ver. 2

sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp

@@ -323,12 +323,8 @@ pcl::SampleConsensusModelCircle2D<PointT>::doSamplesVerifyModel (
 template <typename PointT> bool 
 pcl::SampleConsensusModelCircle2D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
 {
-  // Needs a valid model coefficients
-  if (model_coefficients.size () != 3)
-  {
-    PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+  if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
     return (false);
-  }
 
   if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[2] < radius_min_)
     return (false);

sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp

@@ -439,12 +439,8 @@ pcl::SampleConsensusModelCircle3D<PointT>::doSamplesVerifyModel (
 template <typename PointT> bool
 pcl::SampleConsensusModelCircle3D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
 {
-  // Needs a valid model coefficients
-  if (model_coefficients.size () != 7)
-  {
-    PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+  if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
     return (false);
-  }
 
   if (radius_min_ != -DBL_MAX && model_coefficients[3] < radius_min_)
     return (false);

sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp

@@ -496,13 +496,9 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::pointToAxisDistance (
 template <typename PointT, typename PointNT> bool 
 pcl::SampleConsensusModelCone<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
 {
-  // Needs a valid model coefficients
-  if (model_coefficients.size () != 7)
-  {
-    PCL_ERROR ("[pcl::SampleConsensusModelCone::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+  if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
     return (false);
-  }
-
+
   // Check against template, if given
   if (eps_angle_ > 0.0)
   {

sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp

@@ -443,13 +443,9 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPointToCylinder (
 template <typename PointT, typename PointNT> bool 
 pcl::SampleConsensusModelCylinder<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
 {
-  // Needs a valid model coefficients
-  if (model_coefficients.size () != 7)
-  {
-    PCL_ERROR ("[pcl::SampleConsensusModelCylinder::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+  if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
     return (false);
-  }
-
+
   // Check against template, if given
   if (eps_angle_ > 0.0)
   {

sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp

@@ -47,12 +47,8 @@
 template <typename PointT, typename PointNT> bool
 pcl::SampleConsensusModelNormalParallelPlane<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
 {
-  // Needs a valid model coefficients
-  if (model_coefficients.size () != 4)
-  {
-    PCL_ERROR ("[pcl::SampleConsensusModelNormalParallelPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+  if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
     return (false);
-  }
 
   // Check against template, if given
   if (eps_angle_ > 0.0)

sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp

@@ -209,12 +209,8 @@ pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::getDistancesToModel (
 template <typename PointT, typename PointNT> bool 
 pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
 {
-  // Needs a valid model coefficients
-  if (model_coefficients.size () != 4)
-  {
-    PCL_ERROR ("[pcl::SampleConsensusModelNormalSphere::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+  if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
     return (false);
-  }
 
   if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[3] < radius_min_)
     return (false);

sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp

@@ -90,12 +90,8 @@ pcl::SampleConsensusModelParallelLine<PointT>::getDistancesToModel (
 template <typename PointT> bool
 pcl::SampleConsensusModelParallelLine<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
 {
-  // Needs a valid model coefficients
-  if (model_coefficients.size () != 6)
-  {
-    PCL_ERROR ("[pcl::SampleConsensusParallelLine::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+  if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
     return (false);
-  }
 
   // Check against template, if given
   if (eps_angle_ > 0.0)

sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp

@@ -89,12 +89,8 @@ pcl::SampleConsensusModelParallelPlane<PointT>::getDistancesToModel (
 template <typename PointT> bool
 pcl::SampleConsensusModelParallelPlane<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
 {
-  // Needs a valid model coefficients
-  if (model_coefficients.size () != 4)
-  {
-    PCL_ERROR ("[pcl::SampleConsensusModelParallelPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+  if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
     return (false);
-  }
 
   // Check against template, if given
   if (eps_angle_ > 0.0)

sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp

@@ -89,12 +89,8 @@ pcl::SampleConsensusModelPerpendicularPlane<PointT>::getDistancesToModel (
 template <typename PointT> bool
 pcl::SampleConsensusModelPerpendicularPlane<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
 {
-  // Needs a valid model coefficients
-  if (model_coefficients.size () != 4)
-  {
-    PCL_ERROR ("[pcl::SampleConsensusModelPerpendicularPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
+  if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
     return (false);
-  }
 
   // Check against template, if given
   if (eps_angle_ > 0.0)

sample_consensus/include/pcl/sample_consensus/sac_model.h

@@ -365,6 +365,13 @@ namespace pcl
         return (it->second);
       }
 
+      /** \brief Return the number of coefficients in the model. */
+      inline unsigned int
+      getModelSize () const
+      {
+        return model_size_;
+      }
+
       /** \brief Set the minimum and maximum allowable radius limits for the
         * model (applicable to models that estimate a radius)
         * \param[in] min_radius the minimum radius model
@@ -443,6 +450,7 @@ namespace pcl
       }
 
     protected:
+
       /** \brief Fills a sample array with random samples from the indices_ vector
         * \param[out] sample the set of indices of target_ to analyze
         */
@@ -501,10 +509,22 @@ namespace pcl
       }
 
       /** \brief Check whether a model is valid given the user constraints.
+        *
+        * Default implementation verifies that the number of coefficients in the supplied model is as expected for this
+        * SAC model type. Specific SAC models should extend this function by checking the user constraints (if any).
+        *
         * \param[in] model_coefficients the set of model coefficients
         */
-      virtual inline bool
-      isModelValid (const Eigen::VectorXf &model_coefficients) = 0;
+      virtual bool
+      isModelValid (const Eigen::VectorXf &model_coefficients)
+      {
+        if (model_coefficients.size () != model_size_)
+        {
+          PCL_ERROR ("[pcl::%s::isModelValid] Invalid number of model coefficients given (%lu)!\n", getClassName ().c_str (), model_coefficients.size ());
+          return (false);
+        }
+        return (true);
+      }
 
       /** \brief Check if a sample of indices results in a good sample of points
         * indices. Pure virtual.
@@ -551,6 +571,9 @@ namespace pcl
       /** \brief A vector holding the distances to the computed model. Used internally. */
       std::vector<double> error_sqr_dists_;
 
+      /** \brief The number of coefficients in the model. Every subclass should initialize this appropriately. */
+      unsigned int model_size_;
+
       /** \brief Boost-based random number generator. */
       inline int
       rnd ()

sample_consensus/include/pcl/sample_consensus/sac_model_circle.h

@@ -66,6 +66,7 @@ namespace pcl
       using SampleConsensusModel<PointT>::radius_min_;
       using SampleConsensusModel<PointT>::radius_max_;
       using SampleConsensusModel<PointT>::error_sqr_dists_;
+      using SampleConsensusModel<PointT>::isModelValid;
 
       typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
       typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
@@ -81,6 +82,7 @@ namespace pcl
         : SampleConsensusModel<PointT> (cloud, random), tmp_inliers_ () 
       {
         model_name_ = "SampleConsensusModelCircle2D";
+        model_size_ = 3;
       }
 
       /** \brief Constructor for base SampleConsensusModelCircle2D.
@@ -94,6 +96,7 @@ namespace pcl
         : SampleConsensusModel<PointT> (cloud, indices, random), tmp_inliers_ ()
       {
         model_name_ = "SampleConsensusModelCircle2D";
+        model_size_ = 3;
       }
 
       /** \brief Copy constructor.
@@ -195,10 +198,12 @@ namespace pcl
       getModelType () const { return (SACMODEL_CIRCLE2D); }
 
     protected:
+      using SampleConsensusModel<PointT>::model_size_;
+
       /** \brief Check whether a model is valid given the user constraints.
         * \param[in] model_coefficients the set of model coefficients
         */
-      bool 
+      virtual bool
       isModelValid (const Eigen::VectorXf &model_coefficients);
 
       /** \brief Check if a sample of indices results in a good sample of points indices.

sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h

@@ -66,6 +66,7 @@ namespace pcl
       using SampleConsensusModel<PointT>::indices_;
       using SampleConsensusModel<PointT>::radius_min_;
       using SampleConsensusModel<PointT>::radius_max_;
+      using SampleConsensusModel<PointT>::isModelValid;
 
       typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
       typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
@@ -83,6 +84,7 @@ namespace pcl
         : SampleConsensusModel<PointT> (cloud, random)
       {
         model_name_ = "SampleConsensusModelCircle3D";
+        model_size_ = 7;
       }
 
       /** \brief Constructor for base SampleConsensusModelCircle3D.
@@ -96,6 +98,7 @@ namespace pcl
         : SampleConsensusModel<PointT> (cloud, indices, random)
       {
         model_name_ = "SampleConsensusModelCircle3D";
+        model_size_ = 7;
       }
 
       /** \brief Empty destructor */
@@ -197,10 +200,12 @@ namespace pcl
       getModelType () const { return (SACMODEL_CIRCLE3D); }
 
     protected:
+      using SampleConsensusModel<PointT>::model_size_;
+
       /** \brief Check whether a model is valid given the user constraints.
         * \param[in] model_coefficients the set of model coefficients
         */
-      bool
+      virtual bool
       isModelValid (const Eigen::VectorXf &model_coefficients);
 
       /** \brief Check if a sample of indices results in a good sample of points indices.

sample_consensus/include/pcl/sample_consensus/sac_model_cone.h

@@ -74,6 +74,7 @@ namespace pcl
       using SampleConsensusModelFromNormals<PointT, PointNT>::normals_;
       using SampleConsensusModelFromNormals<PointT, PointNT>::normal_distance_weight_;
       using SampleConsensusModel<PointT>::error_sqr_dists_;
+      using SampleConsensusModel<PointT>::isModelValid;
 
       typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
       typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
@@ -95,6 +96,7 @@ namespace pcl
         , tmp_inliers_ ()
       {
         model_name_ = "SampleConsensusModelCone";
+        model_size_ = 7;
       }
 
       /** \brief Constructor for base SampleConsensusModelCone.
@@ -114,6 +116,7 @@ namespace pcl
         , tmp_inliers_ ()
       {
         model_name_ = "SampleConsensusModelCone";
+        model_size_ = 7;
       }
 
       /** \brief Copy constructor.
@@ -268,6 +271,8 @@ namespace pcl
       getModelType () const { return (SACMODEL_CONE); }
 
     protected:
+      using SampleConsensusModel<PointT>::model_size_;
+
       /** \brief Get the distance from a point to a line (represented by a point and a direction)
         * \param[in] pt a point
         * \param[in] model_coefficients the line coefficients (a point on the line, line direction)
@@ -284,7 +289,7 @@ namespace pcl
       /** \brief Check whether a model is valid given the user constraints.
         * \param[in] model_coefficients the set of model coefficients
         */
-      bool 
+      virtual bool
       isModelValid (const Eigen::VectorXf &model_coefficients);
 
       /** \brief Check if a sample of indices results in a good sample of points

sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h

@@ -74,6 +74,7 @@ namespace pcl
       using SampleConsensusModelFromNormals<PointT, PointNT>::normals_;
       using SampleConsensusModelFromNormals<PointT, PointNT>::normal_distance_weight_;
       using SampleConsensusModel<PointT>::error_sqr_dists_;
+      using SampleConsensusModel<PointT>::isModelValid;
 
       typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
       typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
@@ -93,6 +94,7 @@ namespace pcl
         , tmp_inliers_ ()
       {
         model_name_ = "SampleConsensusModelCylinder";
+        model_size_ = 7;
       }
 
       /** \brief Constructor for base SampleConsensusModelCylinder.
@@ -110,6 +112,7 @@ namespace pcl
         , tmp_inliers_ ()
       {
         model_name_ = "SampleConsensusModelCylinder";
+        model_size_ = 7;
       }
 
       /** \brief Copy constructor.
@@ -240,6 +243,8 @@ namespace pcl
       getModelType () const { return (SACMODEL_CYLINDER); }
 
     protected:
+      using SampleConsensusModel<PointT>::model_size_;
+
       /** \brief Get the distance from a point to a line (represented by a point and a direction)
         * \param[in] pt a point
         * \param[in] model_coefficients the line coefficients (a point on the line, line direction)
@@ -284,7 +289,7 @@ namespace pcl
       /** \brief Check whether a model is valid given the user constraints.
         * \param[in] model_coefficients the set of model coefficients
         */
-      bool 
+      virtual bool
       isModelValid (const Eigen::VectorXf &model_coefficients);
 
       /** \brief Check if a sample of indices results in a good sample of points

sample_consensus/include/pcl/sample_consensus/sac_model_line.h

@@ -67,6 +67,7 @@ namespace pcl
       using SampleConsensusModel<PointT>::input_;
       using SampleConsensusModel<PointT>::indices_;
       using SampleConsensusModel<PointT>::error_sqr_dists_;
+      using SampleConsensusModel<PointT>::isModelValid;
 
       typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
       typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
@@ -82,6 +83,7 @@ namespace pcl
         : SampleConsensusModel<PointT> (cloud, random)
       {
         model_name_ = "SampleConsensusModelLine";
+        model_size_ = 6;
       }
 
       /** \brief Constructor for base SampleConsensusModelLine.
@@ -95,6 +97,7 @@ namespace pcl
         : SampleConsensusModel<PointT> (cloud, indices, random)
       {
         model_name_ = "SampleConsensusModelLine";
+        model_size_ = 6;
       }
 
       /** \brief Empty destructor */
@@ -176,20 +179,7 @@ namespace pcl
       getModelType () const { return (SACMODEL_LINE); }
 
     protected:
-      /** \brief Check whether a model is valid given the user constraints.
-        * \param[in] model_coefficients the set of model coefficients
-        */
-      inline bool 
-      isModelValid (const Eigen::VectorXf &model_coefficients)
-      {
-        if (model_coefficients.size () != 6)
-        {
-          PCL_ERROR ("[pcl::SampleConsensusModelLine::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
-          return (false);
-        }
-
-        return (true);
-      }
+      using SampleConsensusModel<PointT>::model_size_;
 
       /** \brief Check if a sample of indices results in a good sample of points
         * indices.