1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2010-2011, Willow Garage, Inc. 6 * Copyright (c) 2012-, Open Perception, Inc. 7 * 8 * All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 14 * * Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * * Redistributions in binary form must reproduce the above 17 * copyright notice, this list of conditions and the following 18 * disclaimer in the documentation and/or other materials provided 19 * with the distribution. 20 * * Neither the name of the copyright holder(s) nor the names of its 21 * contributors may be used to endorse or promote products derived 22 * from this software without specific prior written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 * 37 * $Id$ 38 * 39 */ 40 41 #pragma once 42 43 #ifdef __SSE__ 44 #include <xmmintrin.h> // for __m128 45 #endif // ifdef __SSE__ 46 #ifdef __AVX__ 47 #include <immintrin.h> // for __m256 48 #endif // ifdef __AVX__ 49 50 #include <pcl/sample_consensus/sac_model.h> 51 #include <pcl/sample_consensus/model_types.h> 52 53 namespace pcl 54 { 55 /** \brief SampleConsensusModelSphere defines a model for 3D sphere segmentation. 56 * The model coefficients are defined as: 57 * - \b center.x : the X coordinate of the sphere's center 58 * - \b center.y : the Y coordinate of the sphere's center 59 * - \b center.z : the Z coordinate of the sphere's center 60 * - \b radius : the sphere's radius 61 * 62 * \author Radu B. Rusu 63 * \ingroup sample_consensus 64 */ 65 template <typename PointT> 66 class SampleConsensusModelSphere : public SampleConsensusModel<PointT> 67 { 68 public: 69 using SampleConsensusModel<PointT>::model_name_; 70 using SampleConsensusModel<PointT>::input_; 71 using SampleConsensusModel<PointT>::indices_; 72 using SampleConsensusModel<PointT>::radius_min_; 73 using SampleConsensusModel<PointT>::radius_max_; 74 using SampleConsensusModel<PointT>::error_sqr_dists_; 75 76 using PointCloud = typename SampleConsensusModel<PointT>::PointCloud; 77 using PointCloudPtr = typename SampleConsensusModel<PointT>::PointCloudPtr; 78 using PointCloudConstPtr = typename SampleConsensusModel<PointT>::PointCloudConstPtr; 79 80 using Ptr = shared_ptr<SampleConsensusModelSphere<PointT> >; 81 using ConstPtr = shared_ptr<const SampleConsensusModelSphere<PointT>>; 82 83 /** \brief Constructor for base SampleConsensusModelSphere. 84 * \param[in] cloud the input point cloud dataset 85 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 86 */ 87 SampleConsensusModelSphere (const PointCloudConstPtr &cloud, 88 bool random = false) 89 : SampleConsensusModel<PointT> (cloud, random) 90 { 91 model_name_ = "SampleConsensusModelSphere"; 92 sample_size_ = 4; 93 model_size_ = 4; 94 } 95 96 /** \brief Constructor for base SampleConsensusModelSphere. 97 * \param[in] cloud the input point cloud dataset 98 * \param[in] indices a vector of point indices to be used from \a cloud 99 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 100 */ 101 SampleConsensusModelSphere (const PointCloudConstPtr &cloud, 102 const Indices &indices, 103 bool random = false) 104 : SampleConsensusModel<PointT> (cloud, indices, random) 105 { 106 model_name_ = "SampleConsensusModelSphere"; 107 sample_size_ = 4; 108 model_size_ = 4; 109 } 110 111 /** \brief Empty destructor */ ~SampleConsensusModelSphere()112 ~SampleConsensusModelSphere () {} 113 114 /** \brief Copy constructor. 115 * \param[in] source the model to copy into this 116 */ SampleConsensusModelSphere(const SampleConsensusModelSphere & source)117 SampleConsensusModelSphere (const SampleConsensusModelSphere &source) : 118 SampleConsensusModel<PointT> () 119 { 120 *this = source; 121 model_name_ = "SampleConsensusModelSphere"; 122 } 123 124 /** \brief Copy constructor. 125 * \param[in] source the model to copy into this 126 */ 127 inline SampleConsensusModelSphere& 128 operator = (const SampleConsensusModelSphere &source) 129 { 130 SampleConsensusModel<PointT>::operator=(source); 131 return (*this); 132 } 133 134 /** \brief Check whether the given index samples can form a valid sphere model, compute the model 135 * coefficients from these samples and store them internally in model_coefficients. 136 * The sphere coefficients are: x, y, z, R. 137 * \param[in] samples the point indices found as possible good candidates for creating a valid model 138 * \param[out] model_coefficients the resultant model coefficients 139 */ 140 bool 141 computeModelCoefficients (const Indices &samples, 142 Eigen::VectorXf &model_coefficients) const override; 143 144 /** \brief Compute all distances from the cloud data to a given sphere model. 145 * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to 146 * \param[out] distances the resultant estimated distances 147 */ 148 void 149 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 150 std::vector<double> &distances) const override; 151 152 /** \brief Select all the points which respect the given model coefficients as inliers. 153 * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to 154 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 155 * \param[out] inliers the resultant model inliers 156 */ 157 void 158 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 159 const double threshold, 160 Indices &inliers) override; 161 162 /** \brief Count all the points which respect the given model coefficients as inliers. 163 * 164 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 165 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers 166 * \return the resultant number of inliers 167 */ 168 std::size_t 169 countWithinDistance (const Eigen::VectorXf &model_coefficients, 170 const double threshold) const override; 171 172 /** \brief Recompute the sphere coefficients using the given inlier set and return them to the user. 173 * @note: these are the coefficients of the sphere model after refinement (e.g. after SVD) 174 * \param[in] inliers the data inliers found as supporting the model 175 * \param[in] model_coefficients the initial guess for the optimization 176 * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization 177 */ 178 void 179 optimizeModelCoefficients (const Indices &inliers, 180 const Eigen::VectorXf &model_coefficients, 181 Eigen::VectorXf &optimized_coefficients) const override; 182 183 /** \brief Create a new point cloud with inliers projected onto the sphere model. 184 * \param[in] inliers the data inliers that we want to project on the sphere model 185 * \param[in] model_coefficients the coefficients of a sphere model 186 * \param[out] projected_points the resultant projected points 187 * \param[in] copy_data_fields set to true if we need to copy the other data fields 188 * \todo implement this. 189 */ 190 void 191 projectPoints (const Indices &inliers, 192 const Eigen::VectorXf &model_coefficients, 193 PointCloud &projected_points, 194 bool copy_data_fields = true) const override; 195 196 /** \brief Verify whether a subset of indices verifies the given sphere model coefficients. 197 * \param[in] indices the data indices that need to be tested against the sphere model 198 * \param[in] model_coefficients the sphere model coefficients 199 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 200 */ 201 bool 202 doSamplesVerifyModel (const std::set<index_t> &indices, 203 const Eigen::VectorXf &model_coefficients, 204 const double threshold) const override; 205 206 /** \brief Return a unique id for this model (SACMODEL_SPHERE). */ getModelType()207 inline pcl::SacModel getModelType () const override { return (SACMODEL_SPHERE); } 208 209 protected: 210 using SampleConsensusModel<PointT>::sample_size_; 211 using SampleConsensusModel<PointT>::model_size_; 212 213 /** \brief Check whether a model is valid given the user constraints. 214 * \param[in] model_coefficients the set of model coefficients 215 */ 216 bool isModelValid(const Eigen::VectorXf & model_coefficients)217 isModelValid (const Eigen::VectorXf &model_coefficients) const override 218 { 219 if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients)) 220 return (false); 221 222 if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[3] < radius_min_) 223 return (false); 224 if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[3] > radius_max_) 225 return (false); 226 227 return (true); 228 } 229 230 /** \brief Check if a sample of indices results in a good sample of points 231 * indices. 232 * \param[in] samples the resultant index samples 233 */ 234 bool 235 isSampleGood(const Indices &samples) const override; 236 237 /** This implementation uses no SIMD instructions. It is not intended for normal use. 238 * See countWithinDistance which automatically uses the fastest implementation. 239 */ 240 std::size_t 241 countWithinDistanceStandard (const Eigen::VectorXf &model_coefficients, 242 const double threshold, 243 std::size_t i = 0) const; 244 245 #if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__) 246 /** This implementation uses SSE, SSE2, and SSE4.1 instructions. It is not intended for normal use. 247 * See countWithinDistance which automatically uses the fastest implementation. 248 */ 249 std::size_t 250 countWithinDistanceSSE (const Eigen::VectorXf &model_coefficients, 251 const double threshold, 252 std::size_t i = 0) const; 253 #endif 254 255 #if defined (__AVX__) && defined (__AVX2__) 256 /** This implementation uses AVX and AVX2 instructions. It is not intended for normal use. 257 * See countWithinDistance which automatically uses the fastest implementation. 258 */ 259 std::size_t 260 countWithinDistanceAVX (const Eigen::VectorXf &model_coefficients, 261 const double threshold, 262 std::size_t i = 0) const; 263 #endif 264 265 private: 266 struct OptimizationFunctor : pcl::Functor<float> 267 { 268 /** Functor constructor 269 * \param[in] indices the indices of data points to evaluate 270 * \param[in] estimator pointer to the estimator object 271 */ OptimizationFunctorOptimizationFunctor272 OptimizationFunctor (const pcl::SampleConsensusModelSphere<PointT> *model, const Indices& indices) : 273 pcl::Functor<float> (indices.size ()), model_ (model), indices_ (indices) {} 274 275 /** Cost function to be minimized 276 * \param[in] x the variables array 277 * \param[out] fvec the resultant functions evaluations 278 * \return 0 279 */ 280 int operatorOptimizationFunctor281 operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const 282 { 283 Eigen::Vector4f cen_t; 284 cen_t[3] = 0; 285 for (int i = 0; i < values (); ++i) 286 { 287 // Compute the difference between the center of the sphere and the datapoint X_i 288 cen_t.head<3>() = (*model_->input_)[indices_[i]].getVector3fMap() - x.head<3>(); 289 290 // g = sqrt ((x-a)^2 + (y-b)^2 + (z-c)^2) - R 291 fvec[i] = ::sqrt (cen_t.dot (cen_t)) - x[3]; 292 } 293 return (0); 294 } 295 296 const pcl::SampleConsensusModelSphere<PointT> *model_; 297 const Indices &indices_; 298 }; 299 300 #ifdef __AVX__ 301 inline __m256 sqr_dist8 (const std::size_t i, const __m256 a_vec, const __m256 b_vec, const __m256 c_vec) const; 302 #endif 303 304 #ifdef __SSE__ 305 inline __m128 sqr_dist4 (const std::size_t i, const __m128 a_vec, const __m128 b_vec, const __m128 c_vec) const; 306 #endif 307 }; 308 } 309 310 #ifdef PCL_NO_PRECOMPILE 311 #include <pcl/sample_consensus/impl/sac_model_sphere.hpp> 312 #endif 313