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) 2012-, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 */ 00037 00038 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE3D_H_ 00039 #define PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE3D_H_ 00040 00041 #include <pcl/sample_consensus/sac_model.h> 00042 #include <pcl/sample_consensus/model_types.h> 00043 00044 namespace pcl 00045 { 00046 /** \brief SampleConsensusModelCircle3D defines a model for 3D circle segmentation. 00047 * 00048 * The model coefficients are defined as: 00049 * - \b center.x : the X coordinate of the circle's center 00050 * - \b center.y : the Y coordinate of the circle's center 00051 * - \b center.z : the Z coordinate of the circle's center 00052 * - \b radius : the circle's radius 00053 * - \b normal.x : the X coordinate of the normal's direction 00054 * - \b normal.y : the Y coordinate of the normal's direction 00055 * - \b normal.z : the Z coordinate of the normal's direction 00056 * 00057 * \author Raoul Hoffmann, Karol Hausman, Radu B. Rusu 00058 * \ingroup sample_consensus 00059 */ 00060 template <typename PointT> 00061 class SampleConsensusModelCircle3D : public SampleConsensusModel<PointT> 00062 { 00063 public: 00064 using SampleConsensusModel<PointT>::input_; 00065 using SampleConsensusModel<PointT>::indices_; 00066 using SampleConsensusModel<PointT>::radius_min_; 00067 using SampleConsensusModel<PointT>::radius_max_; 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<SampleConsensusModelCircle3D<PointT> > Ptr; 00074 typedef boost::shared_ptr<const SampleConsensusModelCircle3D<PointT> > ConstPtr; 00075 00076 /** \brief Constructor for base SampleConsensusModelCircle3D. 00077 * \param[in] cloud the input point cloud dataset 00078 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00079 */ 00080 SampleConsensusModelCircle3D (const PointCloudConstPtr &cloud, 00081 bool random = false) 00082 : SampleConsensusModel<PointT> (cloud, random) {}; 00083 00084 /** \brief Constructor for base SampleConsensusModelCircle3D. 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 SampleConsensusModelCircle3D (const PointCloudConstPtr &cloud, 00090 const std::vector<int> &indices, 00091 bool random = false) 00092 : SampleConsensusModel<PointT> (cloud, indices, random) {}; 00093 00094 /** \brief Empty destructor */ 00095 virtual ~SampleConsensusModelCircle3D () {} 00096 00097 /** \brief Copy constructor. 00098 * \param[in] source the model to copy into this 00099 */ 00100 SampleConsensusModelCircle3D (const SampleConsensusModelCircle3D &source) : 00101 SampleConsensusModel<PointT> (), tmp_inliers_ () 00102 { 00103 *this = source; 00104 } 00105 00106 /** \brief Copy constructor. 00107 * \param[in] source the model to copy into this 00108 */ 00109 inline SampleConsensusModelCircle3D& 00110 operator = (const SampleConsensusModelCircle3D &source) 00111 { 00112 SampleConsensusModel<PointT>::operator=(source); 00113 tmp_inliers_ = source.tmp_inliers_; 00114 return (*this); 00115 } 00116 00117 /** \brief Check whether the given index samples can form a valid 2D circle model, compute the model coefficients 00118 * from these samples and store them in model_coefficients. The circle coefficients are: x, y, R. 00119 * \param[in] samples the point indices found as possible good candidates for creating a valid model 00120 * \param[out] model_coefficients the resultant model coefficients 00121 */ 00122 bool 00123 computeModelCoefficients (const std::vector<int> &samples, 00124 Eigen::VectorXf &model_coefficients); 00125 00126 /** \brief Compute all distances from the cloud data to a given 3D circle model. 00127 * \param[in] model_coefficients the coefficients of a 2D circle model that we need to compute distances to 00128 * \param[out] distances the resultant estimated distances 00129 */ 00130 void 00131 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 00132 std::vector<double> &distances); 00133 00134 /** \brief Compute all distances from the cloud data to a given 3D circle model. 00135 * \param[in] model_coefficients the coefficients of a 3D circle model that we need to compute distances to 00136 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00137 * \param[out] inliers the resultant model inliers 00138 */ 00139 void 00140 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 00141 const double threshold, 00142 std::vector<int> &inliers); 00143 00144 /** \brief Count all the points which respect the given model coefficients as inliers. 00145 * 00146 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 00147 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers 00148 * \return the resultant number of inliers 00149 */ 00150 virtual int 00151 countWithinDistance (const Eigen::VectorXf &model_coefficients, 00152 const double threshold); 00153 00154 /** \brief Recompute the 3d circle coefficients using the given inlier set and return them to the user. 00155 * @note: these are the coefficients of the 3d circle model after refinement (eg. after SVD) 00156 * \param[in] inliers the data inliers found as supporting the model 00157 * \param[in] model_coefficients the initial guess for the optimization 00158 * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization 00159 */ 00160 void 00161 optimizeModelCoefficients (const std::vector<int> &inliers, 00162 const Eigen::VectorXf &model_coefficients, 00163 Eigen::VectorXf &optimized_coefficients); 00164 00165 /** \brief Create a new point cloud with inliers projected onto the 3d circle model. 00166 * \param[in] inliers the data inliers that we want to project on the 3d circle model 00167 * \param[in] model_coefficients the coefficients of a 3d circle model 00168 * \param[out] projected_points the resultant projected points 00169 * \param[in] copy_data_fields set to true if we need to copy the other data fields 00170 */ 00171 void 00172 projectPoints (const std::vector<int> &inliers, 00173 const Eigen::VectorXf &model_coefficients, 00174 PointCloud &projected_points, 00175 bool copy_data_fields = true); 00176 00177 /** \brief Verify whether a subset of indices verifies the given 3d circle model coefficients. 00178 * \param[in] indices the data indices that need to be tested against the 3d circle model 00179 * \param[in] model_coefficients the 3d circle model coefficients 00180 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00181 */ 00182 bool 00183 doSamplesVerifyModel (const std::set<int> &indices, 00184 const Eigen::VectorXf &model_coefficients, 00185 const double threshold); 00186 00187 /** \brief Return an unique id for this model (SACMODEL_CIRCLE3D). */ 00188 inline pcl::SacModel 00189 getModelType () const { return (SACMODEL_CIRCLE3D); } 00190 00191 protected: 00192 /** \brief Check whether a model is valid given the user constraints. 00193 * \param[in] model_coefficients the set of model coefficients 00194 */ 00195 bool 00196 isModelValid (const Eigen::VectorXf &model_coefficients); 00197 00198 /** \brief Check if a sample of indices results in a good sample of points indices. 00199 * \param[in] samples the resultant index samples 00200 */ 00201 bool 00202 isSampleGood(const std::vector<int> &samples) const; 00203 00204 private: 00205 /** \brief Temporary pointer to a list of given indices for optimizeModelCoefficients () */ 00206 const std::vector<int> *tmp_inliers_; 00207 00208 /** \brief Functor for the optimization function */ 00209 struct OptimizationFunctor : pcl::Functor<double> 00210 { 00211 /** Functor constructor 00212 * \param[in] m_data_points the number of functions 00213 * \param[in] estimator pointer to the estimator object 00214 * \param[in] distance distance computation function pointer 00215 */ 00216 OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCircle3D<PointT> *model) : 00217 pcl::Functor<double> (m_data_points), model_ (model) {} 00218 00219 /** Cost function to be minimized 00220 * \param[in] x the variables array 00221 * \param[out] fvec the resultant functions evaluations 00222 * \return 0 00223 */ 00224 int operator() (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const 00225 { 00226 for (int i = 0; i < values (); ++i) 00227 { 00228 // what i have: 00229 // P : Sample Point 00230 Eigen::Vector3d P (model_->input_->points[(*model_->tmp_inliers_)[i]].x, model_->input_->points[(*model_->tmp_inliers_)[i]].y, model_->input_->points[(*model_->tmp_inliers_)[i]].z); 00231 // C : Circle Center 00232 Eigen::Vector3d C (x[0], x[1], x[2]); 00233 // N : Circle (Plane) Normal 00234 Eigen::Vector3d N (x[4], x[5], x[6]); 00235 // r : Radius 00236 double r = x[3]; 00237 00238 Eigen::Vector3d helperVectorPC = P - C; 00239 // 1.1. get line parameter 00240 //float lambda = (helperVectorPC.dot(N)) / N.squaredNorm() ; 00241 double lambda = (-(helperVectorPC.dot (N))) / N.dot (N); 00242 // Projected Point on plane 00243 Eigen::Vector3d P_proj = P + lambda * N; 00244 Eigen::Vector3d helperVectorP_projC = P_proj - C; 00245 00246 // K : Point on Circle 00247 Eigen::Vector3d K = C + r * helperVectorP_projC.normalized (); 00248 Eigen::Vector3d distanceVector = P - K; 00249 00250 fvec[i] = distanceVector.norm (); 00251 } 00252 return (0); 00253 } 00254 00255 pcl::SampleConsensusModelCircle3D<PointT> *model_; 00256 }; 00257 }; 00258 } 00259 00260 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE3D_H_