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_CYLINDER_H_ 00042 #define PCL_SAMPLE_CONSENSUS_MODEL_CYLINDER_H_ 00043 00044 #include <pcl/sample_consensus/sac_model.h> 00045 #include <pcl/sample_consensus/model_types.h> 00046 #include <pcl/common/common.h> 00047 #include <pcl/common/distances.h> 00048 00049 namespace pcl 00050 { 00051 /** \brief @b SampleConsensusModelCylinder defines a model for 3D cylinder segmentation. 00052 * The model coefficients are defined as: 00053 * - \b point_on_axis.x : the X coordinate of a point located on the cylinder axis 00054 * - \b point_on_axis.y : the Y coordinate of a point located on the cylinder axis 00055 * - \b point_on_axis.z : the Z coordinate of a point located on the cylinder axis 00056 * - \b axis_direction.x : the X coordinate of the cylinder's axis direction 00057 * - \b axis_direction.y : the Y coordinate of the cylinder's axis direction 00058 * - \b axis_direction.z : the Z coordinate of the cylinder's axis direction 00059 * - \b radius : the cylinder's radius 00060 * 00061 * \author Radu Bogdan Rusu 00062 * \ingroup sample_consensus 00063 */ 00064 template <typename PointT, typename PointNT> 00065 class SampleConsensusModelCylinder : 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<SampleConsensusModelCylinder> Ptr; 00081 00082 /** \brief Constructor for base SampleConsensusModelCylinder. 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 SampleConsensusModelCylinder (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 , tmp_inliers_ () 00092 { 00093 } 00094 00095 /** \brief Constructor for base SampleConsensusModelCylinder. 00096 * \param[in] cloud the input point cloud dataset 00097 * \param[in] indices a vector of point indices to be used from \a cloud 00098 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00099 */ 00100 SampleConsensusModelCylinder (const PointCloudConstPtr &cloud, 00101 const std::vector<int> &indices, 00102 bool random = false) 00103 : SampleConsensusModel<PointT> (cloud, indices, random) 00104 , SampleConsensusModelFromNormals<PointT, PointNT> () 00105 , axis_ (Eigen::Vector3f::Zero ()) 00106 , eps_angle_ (0) 00107 , tmp_inliers_ () 00108 { 00109 } 00110 00111 /** \brief Copy constructor. 00112 * \param[in] source the model to copy into this 00113 */ 00114 SampleConsensusModelCylinder (const SampleConsensusModelCylinder &source) : 00115 SampleConsensusModel<PointT> (), 00116 SampleConsensusModelFromNormals<PointT, PointNT> (), 00117 axis_ (Eigen::Vector3f::Zero ()), 00118 eps_angle_ (0), 00119 tmp_inliers_ () 00120 { 00121 *this = source; 00122 } 00123 00124 /** \brief Empty destructor */ 00125 virtual ~SampleConsensusModelCylinder () {} 00126 00127 /** \brief Copy constructor. 00128 * \param[in] source the model to copy into this 00129 */ 00130 inline SampleConsensusModelCylinder& 00131 operator = (const SampleConsensusModelCylinder &source) 00132 { 00133 SampleConsensusModel<PointT>::operator=(source); 00134 axis_ = source.axis_; 00135 eps_angle_ = source.eps_angle_; 00136 tmp_inliers_ = source.tmp_inliers_; 00137 return (*this); 00138 } 00139 00140 /** \brief Set the angle epsilon (delta) threshold. 00141 * \param[in] ea the maximum allowed difference between the cyilinder axis and the given axis. 00142 */ 00143 inline void 00144 setEpsAngle (const double ea) { eps_angle_ = ea; } 00145 00146 /** \brief Get the angle epsilon (delta) threshold. */ 00147 inline double 00148 getEpsAngle () { return (eps_angle_); } 00149 00150 /** \brief Set the axis along which we need to search for a cylinder direction. 00151 * \param[in] ax the axis along which we need to search for a cylinder direction 00152 */ 00153 inline void 00154 setAxis (const Eigen::Vector3f &ax) { axis_ = ax; } 00155 00156 /** \brief Get the axis along which we need to search for a cylinder direction. */ 00157 inline Eigen::Vector3f 00158 getAxis () { return (axis_); } 00159 00160 /** \brief Check whether the given index samples can form a valid cylinder model, compute the model coefficients 00161 * from these samples and store them in model_coefficients. The cylinder coefficients are: point_on_axis, 00162 * axis_direction, cylinder_radius_R 00163 * \param[in] samples the point indices found as possible good candidates for creating a valid model 00164 * \param[out] model_coefficients the resultant model coefficients 00165 */ 00166 bool 00167 computeModelCoefficients (const std::vector<int> &samples, 00168 Eigen::VectorXf &model_coefficients); 00169 00170 /** \brief Compute all distances from the cloud data to a given cylinder model. 00171 * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to 00172 * \param[out] distances the resultant estimated distances 00173 */ 00174 void 00175 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 00176 std::vector<double> &distances); 00177 00178 /** \brief Select all the points which respect the given model coefficients as inliers. 00179 * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to 00180 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00181 * \param[out] inliers the resultant model inliers 00182 */ 00183 void 00184 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 00185 const double threshold, 00186 std::vector<int> &inliers); 00187 00188 /** \brief Count all the points which respect the given model coefficients as inliers. 00189 * 00190 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 00191 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers 00192 * \return the resultant number of inliers 00193 */ 00194 virtual int 00195 countWithinDistance (const Eigen::VectorXf &model_coefficients, 00196 const double threshold); 00197 00198 /** \brief Recompute the cylinder coefficients using the given inlier set and return them to the user. 00199 * @note: these are the coefficients of the cylinder model after refinement (eg. after SVD) 00200 * \param[in] inliers the data inliers found as supporting the model 00201 * \param[in] model_coefficients the initial guess for the optimization 00202 * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization 00203 */ 00204 void 00205 optimizeModelCoefficients (const std::vector<int> &inliers, 00206 const Eigen::VectorXf &model_coefficients, 00207 Eigen::VectorXf &optimized_coefficients); 00208 00209 00210 /** \brief Create a new point cloud with inliers projected onto the cylinder model. 00211 * \param[in] inliers the data inliers that we want to project on the cylinder model 00212 * \param[in] model_coefficients the coefficients of a cylinder model 00213 * \param[out] projected_points the resultant projected points 00214 * \param[in] copy_data_fields set to true if we need to copy the other data fields 00215 */ 00216 void 00217 projectPoints (const std::vector<int> &inliers, 00218 const Eigen::VectorXf &model_coefficients, 00219 PointCloud &projected_points, 00220 bool copy_data_fields = true); 00221 00222 /** \brief Verify whether a subset of indices verifies the given cylinder model coefficients. 00223 * \param[in] indices the data indices that need to be tested against the cylinder model 00224 * \param[in] model_coefficients the cylinder model coefficients 00225 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00226 */ 00227 bool 00228 doSamplesVerifyModel (const std::set<int> &indices, 00229 const Eigen::VectorXf &model_coefficients, 00230 const double threshold); 00231 00232 /** \brief Return an unique id for this model (SACMODEL_CYLINDER). */ 00233 inline pcl::SacModel 00234 getModelType () const { return (SACMODEL_CYLINDER); } 00235 00236 protected: 00237 /** \brief Get the distance from a point to a line (represented by a point and a direction) 00238 * \param[in] pt a point 00239 * \param[in] model_coefficients the line coefficients (a point on the line, line direction) 00240 */ 00241 double 00242 pointToLineDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients); 00243 00244 /** \brief Project a point onto a line given by a point and a direction vector 00245 * \param[in] pt the input point to project 00246 * \param[in] line_pt the point on the line (make sure that line_pt[3] = 0 as there are no internal checks!) 00247 * \param[in] line_dir the direction of the line (make sure that line_dir[3] = 0 as there are no internal checks!) 00248 * \param[out] pt_proj the resultant projected point 00249 */ 00250 inline void 00251 projectPointToLine (const Eigen::Vector4f &pt, 00252 const Eigen::Vector4f &line_pt, 00253 const Eigen::Vector4f &line_dir, 00254 Eigen::Vector4f &pt_proj) 00255 { 00256 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); 00257 // Calculate the projection of the point on the line 00258 pt_proj = line_pt + k * line_dir; 00259 } 00260 00261 /** \brief Project a point onto a cylinder given by its model coefficients (point_on_axis, axis_direction, 00262 * cylinder_radius_R) 00263 * \param[in] pt the input point to project 00264 * \param[in] model_coefficients the coefficients of the cylinder (point_on_axis, axis_direction, cylinder_radius_R) 00265 * \param[out] pt_proj the resultant projected point 00266 */ 00267 void 00268 projectPointToCylinder (const Eigen::Vector4f &pt, 00269 const Eigen::VectorXf &model_coefficients, 00270 Eigen::Vector4f &pt_proj); 00271 00272 /** \brief Get a string representation of the name of this class. */ 00273 std::string 00274 getName () const { return ("SampleConsensusModelCylinder"); } 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 temporary pointer to a list of given indices for optimizeModelCoefficients () */ 00298 const std::vector<int> *tmp_inliers_; 00299 00300 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 00301 #pragma GCC diagnostic ignored "-Weffc++" 00302 #endif 00303 /** \brief Functor for the optimization function */ 00304 struct OptimizationFunctor : pcl::Functor<float> 00305 { 00306 /** Functor constructor 00307 * \param[in] m_data_points the number of data points to evaluate 00308 * \param[in] estimator pointer to the estimator object 00309 * \param[in] distance distance computation function pointer 00310 */ 00311 OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCylinder<PointT, PointNT> *model) : 00312 pcl::Functor<float> (m_data_points), model_ (model) {} 00313 00314 /** Cost function to be minimized 00315 * \param[in] x variables array 00316 * \param[out] fvec resultant functions evaluations 00317 * \return 0 00318 */ 00319 int 00320 operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const 00321 { 00322 Eigen::Vector4f line_pt (x[0], x[1], x[2], 0); 00323 Eigen::Vector4f line_dir (x[3], x[4], x[5], 0); 00324 00325 for (int i = 0; i < values (); ++i) 00326 { 00327 // dist = f - r 00328 Eigen::Vector4f pt (model_->input_->points[(*model_->tmp_inliers_)[i]].x, 00329 model_->input_->points[(*model_->tmp_inliers_)[i]].y, 00330 model_->input_->points[(*model_->tmp_inliers_)[i]].z, 0); 00331 00332 fvec[i] = static_cast<float> (pcl::sqrPointToLineDistance (pt, line_pt, line_dir) - x[6]*x[6]); 00333 } 00334 return (0); 00335 } 00336 00337 pcl::SampleConsensusModelCylinder<PointT, PointNT> *model_; 00338 }; 00339 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 00340 #pragma GCC diagnostic warning "-Weffc++" 00341 #endif 00342 }; 00343 } 00344 00345 #ifdef PCL_NO_PRECOMPILE 00346 #include <pcl/sample_consensus/impl/sac_model_cylinder.hpp> 00347 #endif 00348 00349 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CYLINDER_H_