Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, 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$ 00038 * 00039 */ 00040 00041 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_NORMALPARALLELPLANE_H_ 00042 #define PCL_SAMPLE_CONSENSUS_MODEL_NORMALPARALLELPLANE_H_ 00043 00044 #include <pcl/sample_consensus/sac_model.h> 00045 #include <pcl/sample_consensus/sac_model_plane.h> 00046 #include <pcl/sample_consensus/sac_model_perpendicular_plane.h> 00047 #include <pcl/sample_consensus/model_types.h> 00048 00049 namespace pcl 00050 { 00051 /** \brief SampleConsensusModelNormalParallelPlane defines a model for 3D 00052 * plane segmentation using additional surface normal constraints. Basically 00053 * this 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 plane's normal and the inlier points normals. In addition, 00056 * the plane normal must lie parallel to an user-specified axis. 00057 * 00058 * The model coefficients are defined as: 00059 * - \b a : the X coordinate of the plane's normal (normalized) 00060 * - \b b : the Y coordinate of the plane's normal (normalized) 00061 * - \b c : the Z coordinate of the plane's normal (normalized) 00062 * - \b d : the fourth <a href="http://mathworld.wolfram.com/HessianNormalForm.html">Hessian component</a> of the plane's equation 00063 * 00064 * To set the influence of the surface normals in the inlier estimation 00065 * process, set the normal weight (0.0-1.0), e.g.: 00066 * \code 00067 * SampleConsensusModelNormalPlane<pcl::PointXYZ, pcl::Normal> sac_model; 00068 * ... 00069 * sac_model.setNormalDistanceWeight (0.1); 00070 * ... 00071 * \endcode 00072 * 00073 * In addition, the user can specify more constraints, such as: 00074 * 00075 * - an axis along which we need to search for a plane perpendicular to (\ref setAxis); 00076 * - an angle \a tolerance threshold between the plane's normal and the above given axis (\ref setEpsAngle); 00077 * - a distance we expect the plane to be from the origin (\ref setDistanceFromOrigin); 00078 * - a distance \a tolerance as the maximum allowed deviation from the above given distance from the origin (\ref setEpsDist). 00079 * 00080 * \note Please remember that you need to specify an angle > 0 in order to activate the axis-angle constraint! 00081 * 00082 * \author Radu B. Rusu and Jared Glover and Nico Blodow 00083 * \ingroup sample_consensus 00084 */ 00085 template <typename PointT, typename PointNT> 00086 class SampleConsensusModelNormalParallelPlane : public SampleConsensusModelPlane<PointT>, public SampleConsensusModelFromNormals<PointT, PointNT> 00087 { 00088 public: 00089 using SampleConsensusModel<PointT>::input_; 00090 using SampleConsensusModel<PointT>::indices_; 00091 using SampleConsensusModelFromNormals<PointT, PointNT>::normals_; 00092 using SampleConsensusModelFromNormals<PointT, PointNT>::normal_distance_weight_; 00093 using SampleConsensusModel<PointT>::error_sqr_dists_; 00094 00095 typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud; 00096 typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr; 00097 typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr; 00098 00099 typedef typename SampleConsensusModelFromNormals<PointT, PointNT>::PointCloudNPtr PointCloudNPtr; 00100 typedef typename SampleConsensusModelFromNormals<PointT, PointNT>::PointCloudNConstPtr PointCloudNConstPtr; 00101 00102 typedef boost::shared_ptr<SampleConsensusModelNormalParallelPlane> Ptr; 00103 00104 /** \brief Constructor for base SampleConsensusModelNormalParallelPlane. 00105 * \param[in] cloud the input point cloud dataset 00106 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00107 */ 00108 SampleConsensusModelNormalParallelPlane (const PointCloudConstPtr &cloud, 00109 bool random = false) 00110 : SampleConsensusModelPlane<PointT> (cloud, random) 00111 , SampleConsensusModelFromNormals<PointT, PointNT> () 00112 , axis_ (Eigen::Vector4f::Zero ()) 00113 , distance_from_origin_ (0) 00114 , eps_angle_ (-1.0) 00115 , cos_angle_ (-1.0) 00116 , eps_dist_ (0.0) 00117 { 00118 } 00119 00120 /** \brief Constructor for base SampleConsensusModelNormalParallelPlane. 00121 * \param[in] cloud the input point cloud dataset 00122 * \param[in] indices a vector of point indices to be used from \a cloud 00123 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00124 */ 00125 SampleConsensusModelNormalParallelPlane (const PointCloudConstPtr &cloud, 00126 const std::vector<int> &indices, 00127 bool random = false) 00128 : SampleConsensusModelPlane<PointT> (cloud, indices, random) 00129 , SampleConsensusModelFromNormals<PointT, PointNT> () 00130 , axis_ (Eigen::Vector4f::Zero ()) 00131 , distance_from_origin_ (0) 00132 , eps_angle_ (-1.0) 00133 , cos_angle_ (-1.0) 00134 , eps_dist_ (0.0) 00135 { 00136 } 00137 00138 /** \brief Empty destructor */ 00139 virtual ~SampleConsensusModelNormalParallelPlane () {} 00140 00141 /** \brief Set the axis along which we need to search for a plane perpendicular to. 00142 * \param[in] ax the axis along which we need to search for a plane perpendicular to 00143 */ 00144 inline void 00145 setAxis (const Eigen::Vector3f &ax) { axis_.head<3> () = ax; axis_.normalize ();} 00146 00147 /** \brief Get the axis along which we need to search for a plane perpendicular to. */ 00148 inline Eigen::Vector3f 00149 getAxis () { return (axis_.head<3> ()); } 00150 00151 /** \brief Set the angle epsilon (delta) threshold. 00152 * \param[in] ea the maximum allowed deviation from 90 degrees between the plane normal and the given axis. 00153 * \note You need to specify an angle > 0 in order to activate the axis-angle constraint! 00154 */ 00155 inline void 00156 setEpsAngle (const double ea) { eps_angle_ = ea; cos_angle_ = fabs (cos (ea));} 00157 00158 /** \brief Get the angle epsilon (delta) threshold. */ 00159 inline double 00160 getEpsAngle () { return (eps_angle_); } 00161 00162 /** \brief Set the distance we expect the plane to be from the origin 00163 * \param[in] d distance from the template plane to the origin 00164 */ 00165 inline void 00166 setDistanceFromOrigin (const double d) { distance_from_origin_ = d; } 00167 00168 /** \brief Get the distance of the plane from the origin. */ 00169 inline double 00170 getDistanceFromOrigin () { return (distance_from_origin_); } 00171 00172 /** \brief Set the distance epsilon (delta) threshold. 00173 * \param[in] delta the maximum allowed deviation from the template distance from the origin 00174 */ 00175 inline void 00176 setEpsDist (const double delta) { eps_dist_ = delta; } 00177 00178 /** \brief Get the distance epsilon (delta) threshold. */ 00179 inline double 00180 getEpsDist () { return (eps_dist_); } 00181 00182 /** \brief Select all the points which respect the given model coefficients as inliers. 00183 * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to 00184 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00185 * \param[out] inliers the resultant model inliers 00186 */ 00187 void 00188 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 00189 const double threshold, 00190 std::vector<int> &inliers); 00191 00192 /** \brief Count all the points which respect the given model coefficients as inliers. 00193 * 00194 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 00195 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers 00196 * \return the resultant number of inliers 00197 */ 00198 virtual int 00199 countWithinDistance (const Eigen::VectorXf &model_coefficients, 00200 const double threshold); 00201 00202 /** \brief Compute all distances from the cloud data to a given plane model. 00203 * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to 00204 * \param[out] distances the resultant estimated distances 00205 */ 00206 void 00207 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 00208 std::vector<double> &distances); 00209 00210 /** \brief Return an unique id for this model (SACMODEL_NORMAL_PARALLEL_PLANE). */ 00211 inline pcl::SacModel 00212 getModelType () const { return (SACMODEL_NORMAL_PARALLEL_PLANE); } 00213 00214 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00215 00216 protected: 00217 /** \brief Check whether a model is valid given the user constraints. 00218 * \param[in] model_coefficients the set of model coefficients 00219 */ 00220 bool 00221 isModelValid (const Eigen::VectorXf &model_coefficients); 00222 00223 private: 00224 /** \brief The axis along which we need to search for a plane perpendicular to. */ 00225 Eigen::Vector4f axis_; 00226 00227 /** \brief The distance from the template plane to the origin. */ 00228 double distance_from_origin_; 00229 00230 /** \brief The maximum allowed difference between the plane normal and the given axis. */ 00231 double eps_angle_; 00232 00233 /** \brief The cosine of the angle*/ 00234 double cos_angle_; 00235 /** \brief The maximum allowed deviation from the template distance from the origin. */ 00236 double eps_dist_; 00237 }; 00238 } 00239 00240 #ifdef PCL_NO_PRECOMPILE 00241 #include <pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp> 00242 #endif 00243 00244 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_NORMALPARALLELPLANE_H_