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: sac_model_line.h 2326 2011-08-31 07:48:25Z rusu $ 00038 * 00039 */ 00040 00041 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_STICK_H_ 00042 #define PCL_SAMPLE_CONSENSUS_MODEL_STICK_H_ 00043 00044 #include <pcl/sample_consensus/sac_model.h> 00045 #include <pcl/sample_consensus/model_types.h> 00046 #include <pcl/common/eigen.h> 00047 00048 namespace pcl 00049 { 00050 /** \brief SampleConsensusModelStick defines a model for 3D stick segmentation. 00051 * A stick is a line with an user given minimum/maximum width. 00052 * The model coefficients are defined as: 00053 * - \b point_on_line.x : the X coordinate of a point on the line 00054 * - \b point_on_line.y : the Y coordinate of a point on the line 00055 * - \b point_on_line.z : the Z coordinate of a point on the line 00056 * - \b line_direction.x : the X coordinate of a line's direction 00057 * - \b line_direction.y : the Y coordinate of a line's direction 00058 * - \b line_direction.z : the Z coordinate of a line's direction 00059 * - \b line_width : the width of the line 00060 * \author Radu B. Rusu 00061 * \ingroup sample_consensus 00062 */ 00063 template <typename PointT> 00064 class SampleConsensusModelStick : public SampleConsensusModel<PointT> 00065 { 00066 public: 00067 using SampleConsensusModel<PointT>::input_; 00068 using SampleConsensusModel<PointT>::indices_; 00069 using SampleConsensusModel<PointT>::radius_min_; 00070 using SampleConsensusModel<PointT>::radius_max_; 00071 using SampleConsensusModel<PointT>::error_sqr_dists_; 00072 00073 typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud; 00074 typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr; 00075 typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr; 00076 00077 typedef boost::shared_ptr<SampleConsensusModelStick> Ptr; 00078 00079 /** \brief Constructor for base SampleConsensusModelStick. 00080 * \param[in] cloud the input point cloud dataset 00081 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00082 */ 00083 SampleConsensusModelStick (const PointCloudConstPtr &cloud, 00084 bool random = false) 00085 : SampleConsensusModel<PointT> (cloud, random) {}; 00086 00087 /** \brief Constructor for base SampleConsensusModelStick. 00088 * \param[in] cloud the input point cloud dataset 00089 * \param[in] indices a vector of point indices to be used from \a cloud 00090 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00091 */ 00092 SampleConsensusModelStick (const PointCloudConstPtr &cloud, 00093 const std::vector<int> &indices, 00094 bool random = false) 00095 : SampleConsensusModel<PointT> (cloud, indices, random) {}; 00096 00097 /** \brief Empty destructor */ 00098 virtual ~SampleConsensusModelStick () {} 00099 00100 /** \brief Check whether the given index samples can form a valid stick model, compute the model coefficients from 00101 * these samples and store them internally in model_coefficients_. The stick coefficients are represented by a 00102 * point and a line direction 00103 * \param[in] samples the point indices found as possible good candidates for creating a valid model 00104 * \param[out] model_coefficients the resultant model coefficients 00105 */ 00106 bool 00107 computeModelCoefficients (const std::vector<int> &samples, 00108 Eigen::VectorXf &model_coefficients); 00109 00110 /** \brief Compute all squared distances from the cloud data to a given stick model. 00111 * \param[in] model_coefficients the coefficients of a stick model that we need to compute distances to 00112 * \param[out] distances the resultant estimated squared distances 00113 */ 00114 void 00115 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 00116 std::vector<double> &distances); 00117 00118 /** \brief Select all the points which respect the given model coefficients as inliers. 00119 * \param[in] model_coefficients the coefficients of a stick model that we need to compute distances to 00120 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00121 * \param[out] inliers the resultant model inliers 00122 */ 00123 void 00124 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 00125 const double threshold, 00126 std::vector<int> &inliers); 00127 00128 /** \brief Count all the points which respect the given model coefficients as inliers. 00129 * 00130 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 00131 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers 00132 * \return the resultant number of inliers 00133 */ 00134 virtual int 00135 countWithinDistance (const Eigen::VectorXf &model_coefficients, 00136 const double threshold); 00137 00138 /** \brief Recompute the stick coefficients using the given inlier set and return them to the user. 00139 * @note: these are the coefficients of the stick model after refinement (eg. after SVD) 00140 * \param[in] inliers the data inliers found as supporting the model 00141 * \param[in] model_coefficients the initial guess for the model coefficients 00142 * \param[out] optimized_coefficients the resultant recomputed coefficients after optimization 00143 */ 00144 void 00145 optimizeModelCoefficients (const std::vector<int> &inliers, 00146 const Eigen::VectorXf &model_coefficients, 00147 Eigen::VectorXf &optimized_coefficients); 00148 00149 /** \brief Create a new point cloud with inliers projected onto the stick model. 00150 * \param[in] inliers the data inliers that we want to project on the stick model 00151 * \param[in] model_coefficients the *normalized* coefficients of a stick model 00152 * \param[out] projected_points the resultant projected points 00153 * \param[in] copy_data_fields set to true if we need to copy the other data fields 00154 */ 00155 void 00156 projectPoints (const std::vector<int> &inliers, 00157 const Eigen::VectorXf &model_coefficients, 00158 PointCloud &projected_points, 00159 bool copy_data_fields = true); 00160 00161 /** \brief Verify whether a subset of indices verifies the given stick model coefficients. 00162 * \param[in] indices the data indices that need to be tested against the plane model 00163 * \param[in] model_coefficients the plane model coefficients 00164 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00165 */ 00166 bool 00167 doSamplesVerifyModel (const std::set<int> &indices, 00168 const Eigen::VectorXf &model_coefficients, 00169 const double threshold); 00170 00171 /** \brief Return an unique id for this model (SACMODEL_STACK). */ 00172 inline pcl::SacModel 00173 getModelType () const { return (SACMODEL_STICK); } 00174 00175 protected: 00176 /** \brief Check whether a model is valid given the user constraints. 00177 * \param[in] model_coefficients the set of model coefficients 00178 */ 00179 inline bool 00180 isModelValid (const Eigen::VectorXf &model_coefficients) 00181 { 00182 if (model_coefficients.size () != 7) 00183 { 00184 PCL_ERROR ("[pcl::SampleConsensusModelStick::selectWithinDistance] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00185 return (false); 00186 } 00187 00188 return (true); 00189 } 00190 00191 /** \brief Check if a sample of indices results in a good sample of points 00192 * indices. 00193 * \param[in] samples the resultant index samples 00194 */ 00195 bool 00196 isSampleGood (const std::vector<int> &samples) const; 00197 }; 00198 } 00199 00200 #ifdef PCL_NO_PRECOMPILE 00201 #include <pcl/sample_consensus/impl/sac_model_stick.hpp> 00202 #endif 00203 00204 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_STICK_H_