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_SPHERE_H_ 00042 #define PCL_SAMPLE_CONSENSUS_MODEL_SPHERE_H_ 00043 00044 #include <pcl/sample_consensus/sac_model.h> 00045 #include <pcl/sample_consensus/model_types.h> 00046 00047 namespace pcl 00048 { 00049 /** \brief SampleConsensusModelSphere defines a model for 3D sphere segmentation. 00050 * The model coefficients are defined as: 00051 * - \b center.x : the X coordinate of the sphere's center 00052 * - \b center.y : the Y coordinate of the sphere's center 00053 * - \b center.z : the Z coordinate of the sphere's center 00054 * - \b radius : the sphere's radius 00055 * 00056 * \author Radu B. Rusu 00057 * \ingroup sample_consensus 00058 */ 00059 template <typename PointT> 00060 class SampleConsensusModelSphere : public SampleConsensusModel<PointT> 00061 { 00062 public: 00063 using SampleConsensusModel<PointT>::input_; 00064 using SampleConsensusModel<PointT>::indices_; 00065 using SampleConsensusModel<PointT>::radius_min_; 00066 using SampleConsensusModel<PointT>::radius_max_; 00067 using SampleConsensusModel<PointT>::error_sqr_dists_; 00068 00069 typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud; 00070 typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr; 00071 typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr; 00072 00073 typedef boost::shared_ptr<SampleConsensusModelSphere> Ptr; 00074 00075 /** \brief Constructor for base SampleConsensusModelSphere. 00076 * \param[in] cloud the input point cloud dataset 00077 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00078 */ 00079 SampleConsensusModelSphere (const PointCloudConstPtr &cloud, 00080 bool random = false) 00081 : SampleConsensusModel<PointT> (cloud, random), tmp_inliers_ () 00082 {} 00083 00084 /** \brief Constructor for base SampleConsensusModelSphere. 00085 * \param[in] cloud the input point cloud dataset 00086 * \param[in] indices a vector of point indices to be used from \a cloud 00087 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00088 */ 00089 SampleConsensusModelSphere (const PointCloudConstPtr &cloud, 00090 const std::vector<int> &indices, 00091 bool random = false) 00092 : SampleConsensusModel<PointT> (cloud, indices, random), tmp_inliers_ () 00093 {} 00094 00095 /** \brief Empty destructor */ 00096 virtual ~SampleConsensusModelSphere () {} 00097 00098 /** \brief Copy constructor. 00099 * \param[in] source the model to copy into this 00100 */ 00101 SampleConsensusModelSphere (const SampleConsensusModelSphere &source) : 00102 SampleConsensusModel<PointT> (), tmp_inliers_ () 00103 { 00104 *this = source; 00105 } 00106 00107 /** \brief Copy constructor. 00108 * \param[in] source the model to copy into this 00109 */ 00110 inline SampleConsensusModelSphere& 00111 operator = (const SampleConsensusModelSphere &source) 00112 { 00113 SampleConsensusModel<PointT>::operator=(source); 00114 tmp_inliers_ = source.tmp_inliers_; 00115 return (*this); 00116 } 00117 00118 /** \brief Check whether the given index samples can form a valid sphere model, compute the model 00119 * coefficients from these samples and store them internally in model_coefficients. 00120 * The sphere coefficients are: x, y, z, R. 00121 * \param[in] samples the point indices found as possible good candidates for creating a valid model 00122 * \param[out] model_coefficients the resultant model coefficients 00123 */ 00124 bool 00125 computeModelCoefficients (const std::vector<int> &samples, 00126 Eigen::VectorXf &model_coefficients); 00127 00128 /** \brief Compute all distances from the cloud data to a given sphere model. 00129 * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to 00130 * \param[out] distances the resultant estimated distances 00131 */ 00132 void 00133 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 00134 std::vector<double> &distances); 00135 00136 /** \brief Select all the points which respect the given model coefficients as inliers. 00137 * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to 00138 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00139 * \param[out] inliers the resultant model inliers 00140 */ 00141 void 00142 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 00143 const double threshold, 00144 std::vector<int> &inliers); 00145 00146 /** \brief Count all the points which respect the given model coefficients as inliers. 00147 * 00148 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 00149 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers 00150 * \return the resultant number of inliers 00151 */ 00152 virtual int 00153 countWithinDistance (const Eigen::VectorXf &model_coefficients, 00154 const double threshold); 00155 00156 /** \brief Recompute the sphere coefficients using the given inlier set and return them to the user. 00157 * @note: these are the coefficients of the sphere model after refinement (eg. after SVD) 00158 * \param[in] inliers the data inliers found as supporting the model 00159 * \param[in] model_coefficients the initial guess for the optimization 00160 * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization 00161 */ 00162 void 00163 optimizeModelCoefficients (const std::vector<int> &inliers, 00164 const Eigen::VectorXf &model_coefficients, 00165 Eigen::VectorXf &optimized_coefficients); 00166 00167 /** \brief Create a new point cloud with inliers projected onto the sphere model. 00168 * \param[in] inliers the data inliers that we want to project on the sphere model 00169 * \param[in] model_coefficients the coefficients of a sphere model 00170 * \param[out] projected_points the resultant projected points 00171 * \param[in] copy_data_fields set to true if we need to copy the other data fields 00172 * \todo implement this. 00173 */ 00174 void 00175 projectPoints (const std::vector<int> &inliers, 00176 const Eigen::VectorXf &model_coefficients, 00177 PointCloud &projected_points, 00178 bool copy_data_fields = true); 00179 00180 /** \brief Verify whether a subset of indices verifies the given sphere model coefficients. 00181 * \param[in] indices the data indices that need to be tested against the sphere model 00182 * \param[in] model_coefficients the sphere model coefficients 00183 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00184 */ 00185 bool 00186 doSamplesVerifyModel (const std::set<int> &indices, 00187 const Eigen::VectorXf &model_coefficients, 00188 const double threshold); 00189 00190 /** \brief Return an unique id for this model (SACMODEL_SPHERE). */ 00191 inline pcl::SacModel getModelType () const { return (SACMODEL_SPHERE); } 00192 00193 protected: 00194 /** \brief Check whether a model is valid given the user constraints. 00195 * \param[in] model_coefficients the set of model coefficients 00196 */ 00197 inline bool 00198 isModelValid (const Eigen::VectorXf &model_coefficients) 00199 { 00200 // Needs a valid model coefficients 00201 if (model_coefficients.size () != 4) 00202 { 00203 PCL_ERROR ("[pcl::SampleConsensusModelSphere::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00204 return (false); 00205 } 00206 00207 if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[3] < radius_min_) 00208 return (false); 00209 if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[3] > radius_max_) 00210 return (false); 00211 00212 return (true); 00213 } 00214 00215 /** \brief Check if a sample of indices results in a good sample of points 00216 * indices. 00217 * \param[in] samples the resultant index samples 00218 */ 00219 bool 00220 isSampleGood(const std::vector<int> &samples) const; 00221 00222 private: 00223 /** \brief Temporary pointer to a list of given indices for optimizeModelCoefficients () */ 00224 const std::vector<int> *tmp_inliers_; 00225 00226 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 00227 #pragma GCC diagnostic ignored "-Weffc++" 00228 #endif 00229 struct OptimizationFunctor : pcl::Functor<float> 00230 { 00231 /** Functor constructor 00232 * \param[in] m_data_points the number of data points to evaluate 00233 * \param[in] estimator pointer to the estimator object 00234 * \param[in] distance distance computation function pointer 00235 */ 00236 OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelSphere<PointT> *model) : 00237 pcl::Functor<float>(m_data_points), model_ (model) {} 00238 00239 /** Cost function to be minimized 00240 * \param[in] x the variables array 00241 * \param[out] fvec the resultant functions evaluations 00242 * \return 0 00243 */ 00244 int 00245 operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const 00246 { 00247 Eigen::Vector4f cen_t; 00248 cen_t[3] = 0; 00249 for (int i = 0; i < values (); ++i) 00250 { 00251 // Compute the difference between the center of the sphere and the datapoint X_i 00252 cen_t[0] = model_->input_->points[(*model_->tmp_inliers_)[i]].x - x[0]; 00253 cen_t[1] = model_->input_->points[(*model_->tmp_inliers_)[i]].y - x[1]; 00254 cen_t[2] = model_->input_->points[(*model_->tmp_inliers_)[i]].z - x[2]; 00255 00256 // g = sqrt ((x-a)^2 + (y-b)^2 + (z-c)^2) - R 00257 fvec[i] = sqrtf (cen_t.dot (cen_t)) - x[3]; 00258 } 00259 return (0); 00260 } 00261 00262 pcl::SampleConsensusModelSphere<PointT> *model_; 00263 }; 00264 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 00265 #pragma GCC diagnostic warning "-Weffc++" 00266 #endif 00267 }; 00268 } 00269 00270 #ifdef PCL_NO_PRECOMPILE 00271 #include <pcl/sample_consensus/impl/sac_model_sphere.hpp> 00272 #endif 00273 00274 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_SPHERE_H_