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) 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 */ 00038 00039 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_ 00040 #define PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_ 00041 00042 #include <pcl/sample_consensus/sac_model.h> 00043 #include <pcl/sample_consensus/model_types.h> 00044 #include <pcl/common/common.h> 00045 #include <pcl/common/distances.h> 00046 #include <limits.h> 00047 00048 namespace pcl 00049 { 00050 /** \brief @b SampleConsensusModelCone defines a model for 3D cone segmentation. 00051 * The model coefficients are defined as: 00052 * <ul> 00053 * <li><b>apex.x</b> : the X coordinate of cone's apex 00054 * <li><b>apex.y</b> : the Y coordinate of cone's apex 00055 * <li><b>apex.z</b> : the Z coordinate of cone's apex 00056 * <li><b>axis_direction.x</b> : the X coordinate of the cone's axis direction 00057 * <li><b>axis_direction.y</b> : the Y coordinate of the cone's axis direction 00058 * <li><b>axis_direction.z</b> : the Z coordinate of the cone's axis direction 00059 * <li><b>opening_angle</b> : the cone's opening angle 00060 * </ul> 00061 * \author Stefan Schrandt 00062 * \ingroup sample_consensus 00063 */ 00064 template <typename PointT, typename PointNT> 00065 class SampleConsensusModelCone : public SampleConsensusModel<PointT>, public SampleConsensusModelFromNormals<PointT, PointNT> 00066 { 00067 public: 00068 using SampleConsensusModel<PointT>::input_; 00069 using SampleConsensusModel<PointT>::indices_; 00070 using SampleConsensusModel<PointT>::radius_min_; 00071 using SampleConsensusModel<PointT>::radius_max_; 00072 using SampleConsensusModelFromNormals<PointT, PointNT>::normals_; 00073 using SampleConsensusModelFromNormals<PointT, PointNT>::normal_distance_weight_; 00074 using SampleConsensusModel<PointT>::error_sqr_dists_; 00075 00076 typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud; 00077 typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr; 00078 typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr; 00079 00080 typedef boost::shared_ptr<SampleConsensusModelCone> Ptr; 00081 00082 /** \brief Constructor for base SampleConsensusModelCone. 00083 * \param[in] cloud the input point cloud dataset 00084 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00085 */ 00086 SampleConsensusModelCone (const PointCloudConstPtr &cloud, bool random = false) 00087 : SampleConsensusModel<PointT> (cloud, random) 00088 , SampleConsensusModelFromNormals<PointT, PointNT> () 00089 , axis_ (Eigen::Vector3f::Zero ()) 00090 , eps_angle_ (0) 00091 , min_angle_ (-std::numeric_limits<double>::max ()) 00092 , max_angle_ (std::numeric_limits<double>::max ()) 00093 , tmp_inliers_ () 00094 { 00095 } 00096 00097 /** \brief Constructor for base SampleConsensusModelCone. 00098 * \param[in] cloud the input point cloud dataset 00099 * \param[in] indices a vector of point indices to be used from \a cloud 00100 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00101 */ 00102 SampleConsensusModelCone (const PointCloudConstPtr &cloud, 00103 const std::vector<int> &indices, 00104 bool random = false) 00105 : SampleConsensusModel<PointT> (cloud, indices, random) 00106 , SampleConsensusModelFromNormals<PointT, PointNT> () 00107 , axis_ (Eigen::Vector3f::Zero ()) 00108 , eps_angle_ (0) 00109 , min_angle_ (-std::numeric_limits<double>::max ()) 00110 , max_angle_ (std::numeric_limits<double>::max ()) 00111 , tmp_inliers_ () 00112 { 00113 } 00114 00115 /** \brief Copy constructor. 00116 * \param[in] source the model to copy into this 00117 */ 00118 SampleConsensusModelCone (const SampleConsensusModelCone &source) : 00119 SampleConsensusModel<PointT> (), 00120 SampleConsensusModelFromNormals<PointT, PointNT> (), 00121 axis_ (), eps_angle_ (), min_angle_ (), max_angle_ (), tmp_inliers_ () 00122 { 00123 *this = source; 00124 } 00125 00126 /** \brief Empty destructor */ 00127 virtual ~SampleConsensusModelCone () {} 00128 00129 /** \brief Copy constructor. 00130 * \param[in] source the model to copy into this 00131 */ 00132 inline SampleConsensusModelCone& 00133 operator = (const SampleConsensusModelCone &source) 00134 { 00135 SampleConsensusModel<PointT>::operator=(source); 00136 axis_ = source.axis_; 00137 eps_angle_ = source.eps_angle_; 00138 min_angle_ = source.min_angle_; 00139 max_angle_ = source.max_angle_; 00140 tmp_inliers_ = source.tmp_inliers_; 00141 return (*this); 00142 } 00143 00144 /** \brief Set the angle epsilon (delta) threshold. 00145 * \param[in] ea the maximum allowed difference between the cone's axis and the given axis. 00146 */ 00147 inline void 00148 setEpsAngle (double ea) { eps_angle_ = ea; } 00149 00150 /** \brief Get the angle epsilon (delta) threshold. */ 00151 inline double 00152 getEpsAngle () const { return (eps_angle_); } 00153 00154 /** \brief Set the axis along which we need to search for a cone direction. 00155 * \param[in] ax the axis along which we need to search for a cone direction 00156 */ 00157 inline void 00158 setAxis (const Eigen::Vector3f &ax) { axis_ = ax; } 00159 00160 /** \brief Get the axis along which we need to search for a cone direction. */ 00161 inline Eigen::Vector3f 00162 getAxis () const { return (axis_); } 00163 00164 /** \brief Set the minimum and maximum allowable opening angle for a cone model 00165 * given from a user. 00166 * \param[in] min_angle the minimum allwoable opening angle of a cone model 00167 * \param[in] max_angle the maximum allwoable opening angle of a cone model 00168 */ 00169 inline void 00170 setMinMaxOpeningAngle (const double &min_angle, const double &max_angle) 00171 { 00172 min_angle_ = min_angle; 00173 max_angle_ = max_angle; 00174 } 00175 00176 /** \brief Get the opening angle which we need minumum to validate a cone model. 00177 * \param[out] min_angle the minimum allwoable opening angle of a cone model 00178 * \param[out] max_angle the maximum allwoable opening angle of a cone model 00179 */ 00180 inline void 00181 getMinMaxOpeningAngle (double &min_angle, double &max_angle) const 00182 { 00183 min_angle = min_angle_; 00184 max_angle = max_angle_; 00185 } 00186 00187 /** \brief Check whether the given index samples can form a valid cone model, compute the model coefficients 00188 * from these samples and store them in model_coefficients. The cone coefficients are: apex, 00189 * axis_direction, opening_angle. 00190 * \param[in] samples the point indices found as possible good candidates for creating a valid model 00191 * \param[out] model_coefficients the resultant model coefficients 00192 */ 00193 bool 00194 computeModelCoefficients (const std::vector<int> &samples, 00195 Eigen::VectorXf &model_coefficients); 00196 00197 /** \brief Compute all distances from the cloud data to a given cone model. 00198 * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to 00199 * \param[out] distances the resultant estimated distances 00200 */ 00201 void 00202 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 00203 std::vector<double> &distances); 00204 00205 /** \brief Select all the points which respect the given model coefficients as inliers. 00206 * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to 00207 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00208 * \param[out] inliers the resultant model inliers 00209 */ 00210 void 00211 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 00212 const double threshold, 00213 std::vector<int> &inliers); 00214 00215 /** \brief Count all the points which respect the given model coefficients as inliers. 00216 * 00217 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 00218 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers 00219 * \return the resultant number of inliers 00220 */ 00221 virtual int 00222 countWithinDistance (const Eigen::VectorXf &model_coefficients, 00223 const double threshold); 00224 00225 00226 /** \brief Recompute the cone coefficients using the given inlier set and return them to the user. 00227 * @note: these are the coefficients of the cone model after refinement (eg. after SVD) 00228 * \param[in] inliers the data inliers found as supporting the model 00229 * \param[in] model_coefficients the initial guess for the optimization 00230 * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization 00231 */ 00232 void 00233 optimizeModelCoefficients (const std::vector<int> &inliers, 00234 const Eigen::VectorXf &model_coefficients, 00235 Eigen::VectorXf &optimized_coefficients); 00236 00237 00238 /** \brief Create a new point cloud with inliers projected onto the cone model. 00239 * \param[in] inliers the data inliers that we want to project on the cone model 00240 * \param[in] model_coefficients the coefficients of a cone model 00241 * \param[out] projected_points the resultant projected points 00242 * \param[in] copy_data_fields set to true if we need to copy the other data fields 00243 */ 00244 void 00245 projectPoints (const std::vector<int> &inliers, 00246 const Eigen::VectorXf &model_coefficients, 00247 PointCloud &projected_points, 00248 bool copy_data_fields = true); 00249 00250 /** \brief Verify whether a subset of indices verifies the given cone model coefficients. 00251 * \param[in] indices the data indices that need to be tested against the cone model 00252 * \param[in] model_coefficients the cone model coefficients 00253 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00254 */ 00255 bool 00256 doSamplesVerifyModel (const std::set<int> &indices, 00257 const Eigen::VectorXf &model_coefficients, 00258 const double threshold); 00259 00260 /** \brief Return an unique id for this model (SACMODEL_CONE). */ 00261 inline pcl::SacModel 00262 getModelType () const { return (SACMODEL_CONE); } 00263 00264 protected: 00265 /** \brief Get the distance from a point to a line (represented by a point and a direction) 00266 * \param[in] pt a point 00267 * \param[in] model_coefficients the line coefficients (a point on the line, line direction) 00268 */ 00269 double 00270 pointToAxisDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients); 00271 00272 /** \brief Get a string representation of the name of this class. */ 00273 std::string 00274 getName () const { return ("SampleConsensusModelCone"); } 00275 00276 protected: 00277 /** \brief Check whether a model is valid given the user constraints. 00278 * \param[in] model_coefficients the set of model coefficients 00279 */ 00280 bool 00281 isModelValid (const Eigen::VectorXf &model_coefficients); 00282 00283 /** \brief Check if a sample of indices results in a good sample of points 00284 * indices. Pure virtual. 00285 * \param[in] samples the resultant index samples 00286 */ 00287 bool 00288 isSampleGood (const std::vector<int> &samples) const; 00289 00290 private: 00291 /** \brief The axis along which we need to search for a plane perpendicular to. */ 00292 Eigen::Vector3f axis_; 00293 00294 /** \brief The maximum allowed difference between the plane normal and the given axis. */ 00295 double eps_angle_; 00296 00297 /** \brief The minimum and maximum allowed opening angles of valid cone model. */ 00298 double min_angle_; 00299 double max_angle_; 00300 00301 /** \brief temporary pointer to a list of given indices for optimizeModelCoefficients () */ 00302 const std::vector<int> *tmp_inliers_; 00303 00304 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 00305 #pragma GCC diagnostic ignored "-Weffc++" 00306 #endif 00307 /** \brief Functor for the optimization function */ 00308 struct OptimizationFunctor : pcl::Functor<float> 00309 { 00310 /** Functor constructor 00311 * \param[in] m_data_points the number of data points to evaluate 00312 * \param[in] estimator pointer to the estimator object 00313 * \param[in] distance distance computation function pointer 00314 */ 00315 OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCone<PointT, PointNT> *model) : 00316 pcl::Functor<float> (m_data_points), model_ (model) {} 00317 00318 /** Cost function to be minimized 00319 * \param[in] x variables array 00320 * \param[out] fvec resultant functions evaluations 00321 * \return 0 00322 */ 00323 int 00324 operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const 00325 { 00326 Eigen::Vector4f apex (x[0], x[1], x[2], 0); 00327 Eigen::Vector4f axis_dir (x[3], x[4], x[5], 0); 00328 float opening_angle = x[6]; 00329 00330 float apexdotdir = apex.dot (axis_dir); 00331 float dirdotdir = 1.0f / axis_dir.dot (axis_dir); 00332 00333 for (int i = 0; i < values (); ++i) 00334 { 00335 // dist = f - r 00336 Eigen::Vector4f pt (model_->input_->points[(*model_->tmp_inliers_)[i]].x, 00337 model_->input_->points[(*model_->tmp_inliers_)[i]].y, 00338 model_->input_->points[(*model_->tmp_inliers_)[i]].z, 0); 00339 00340 // Calculate the point's projection on the cone axis 00341 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir; 00342 Eigen::Vector4f pt_proj = apex + k * axis_dir; 00343 00344 // Calculate the actual radius of the cone at the level of the projected point 00345 Eigen::Vector4f height = apex-pt_proj; 00346 float actual_cone_radius = tanf (opening_angle) * height.norm (); 00347 00348 fvec[i] = static_cast<float> (pcl::sqrPointToLineDistance (pt, apex, axis_dir) - actual_cone_radius * actual_cone_radius); 00349 } 00350 return (0); 00351 } 00352 00353 pcl::SampleConsensusModelCone<PointT, PointNT> *model_; 00354 }; 00355 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 00356 #pragma GCC diagnostic warning "-Weffc++" 00357 #endif 00358 }; 00359 } 00360 00361 #ifdef PCL_NO_PRECOMPILE 00362 #include <pcl/sample_consensus/impl/sac_model_cone.hpp> 00363 #endif 00364 00365 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_