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