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