Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/sample_consensus/include/pcl/sample_consensus/sac_model_normal_sphere.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2009-2012, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id: sac_model_normal_sphere.h schrandt $
00038  *
00039  */
00040 
00041 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_NORMALSPHERE_H_
00042 #define PCL_SAMPLE_CONSENSUS_MODEL_NORMALSPHERE_H_
00043 
00044 #include <pcl/sample_consensus/sac_model.h>
00045 #include <pcl/sample_consensus/sac_model_sphere.h>
00046 #include <pcl/sample_consensus/model_types.h>
00047 #include <pcl/common/common.h>
00048 
00049 namespace pcl
00050 {
00051   /** \brief @b SampleConsensusModelNormalSphere defines a model for 3D sphere
00052     * segmentation using additional surface normal constraints. Basically this
00053     * means that checking for inliers will not only involve a "distance to
00054     * model" criterion, but also an additional "maximum angular deviation"
00055     * between the sphere's normal and the inlier points normals.
00056     *
00057     * The model coefficients are defined as:
00058     * <ul>
00059     * <li><b>a</b> : the X coordinate of the plane's normal (normalized)
00060     * <li><b>b</b> : the Y coordinate of the plane's normal (normalized)
00061     * <li><b>c</b> : the Z coordinate of the plane's normal (normalized)
00062     * <li><b>d</b> : radius of the sphere
00063     * </ul>
00064     *
00065     * \author Stefan Schrandt
00066     * \ingroup sample_consensus
00067     */
00068   template <typename PointT, typename PointNT>
00069   class SampleConsensusModelNormalSphere : public SampleConsensusModelSphere<PointT>, public SampleConsensusModelFromNormals<PointT, PointNT>
00070   {
00071     public:
00072       using SampleConsensusModel<PointT>::input_;
00073       using SampleConsensusModel<PointT>::indices_;
00074       using SampleConsensusModel<PointT>::radius_min_;
00075       using SampleConsensusModel<PointT>::radius_max_;
00076       using SampleConsensusModelFromNormals<PointT, PointNT>::normals_;
00077       using SampleConsensusModelFromNormals<PointT, PointNT>::normal_distance_weight_;
00078       using SampleConsensusModel<PointT>::error_sqr_dists_;
00079 
00080       typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
00081       typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
00082       typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr;
00083 
00084       typedef typename SampleConsensusModelFromNormals<PointT, PointNT>::PointCloudNPtr PointCloudNPtr;
00085       typedef typename SampleConsensusModelFromNormals<PointT, PointNT>::PointCloudNConstPtr PointCloudNConstPtr;
00086 
00087       typedef boost::shared_ptr<SampleConsensusModelNormalSphere> Ptr;
00088 
00089       /** \brief Constructor for base SampleConsensusModelNormalSphere.
00090         * \param[in] cloud the input point cloud dataset
00091         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00092         */
00093       SampleConsensusModelNormalSphere (const PointCloudConstPtr &cloud, 
00094                                         bool random = false) 
00095         : SampleConsensusModelSphere<PointT> (cloud, random)
00096         , SampleConsensusModelFromNormals<PointT, PointNT> ()
00097       {
00098       }
00099 
00100       /** \brief Constructor for base SampleConsensusModelNormalSphere.
00101         * \param[in] cloud the input point cloud dataset
00102         * \param[in] indices a vector of point indices to be used from \a cloud
00103         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00104         */
00105       SampleConsensusModelNormalSphere (const PointCloudConstPtr &cloud, 
00106                                         const std::vector<int> &indices,
00107                                         bool random = false) 
00108         : SampleConsensusModelSphere<PointT> (cloud, indices, random)
00109         , SampleConsensusModelFromNormals<PointT, PointNT> ()
00110       {
00111       }
00112       
00113       /** \brief Empty destructor */
00114       virtual ~SampleConsensusModelNormalSphere () {}
00115 
00116       /** \brief Select all the points which respect the given model coefficients as inliers.
00117         * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to
00118         * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
00119         * \param[out] inliers the resultant model inliers
00120         */
00121       void 
00122       selectWithinDistance (const Eigen::VectorXf &model_coefficients, 
00123                             const double threshold, 
00124                             std::vector<int> &inliers);
00125 
00126       /** \brief Count all the points which respect the given model coefficients as inliers. 
00127         * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
00128         * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
00129         * \return the resultant number of inliers
00130         */
00131       virtual int
00132       countWithinDistance (const Eigen::VectorXf &model_coefficients, 
00133                            const double threshold);
00134 
00135       /** \brief Compute all distances from the cloud data to a given sphere model.
00136         * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to
00137         * \param[out] distances the resultant estimated distances
00138         */
00139       void 
00140       getDistancesToModel (const Eigen::VectorXf &model_coefficients, 
00141                            std::vector<double> &distances);
00142 
00143       /** \brief Return an unique id for this model (SACMODEL_NORMAL_SPHERE). */
00144       inline pcl::SacModel 
00145       getModelType () const { return (SACMODEL_NORMAL_SPHERE); }
00146 
00147       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00148 
00149     protected:
00150       /** \brief Check whether a model is valid given the user constraints.
00151         * \param[in] model_coefficients the set of model coefficients
00152         */
00153       bool 
00154       isModelValid (const Eigen::VectorXf &model_coefficients);
00155 
00156   };
00157 }
00158 
00159 #ifdef PCL_NO_PRECOMPILE
00160 #include <pcl/sample_consensus/impl/sac_model_normal_sphere.hpp>
00161 #endif
00162 
00163 #endif  //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_NORMALSPHERE_H_