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_PLANE_H_ 00042 #define PCL_SAMPLE_CONSENSUS_MODEL_PLANE_H_ 00043 00044 #include <pcl/sample_consensus/sac_model.h> 00045 #include <pcl/sample_consensus/model_types.h> 00046 00047 namespace pcl 00048 { 00049 00050 /** \brief Project a point on a planar model given by a set of normalized coefficients 00051 * \param[in] p the input point to project 00052 * \param[in] model_coefficients the coefficients of the plane (a, b, c, d) that satisfy ax+by+cz+d=0 00053 * \param[out] q the resultant projected point 00054 */ 00055 template <typename Point> inline void 00056 projectPoint (const Point &p, const Eigen::Vector4f &model_coefficients, Point &q) 00057 { 00058 // Calculate the distance from the point to the plane 00059 Eigen::Vector4f pp (p.x, p.y, p.z, 1); 00060 // use normalized coefficients to calculate the scalar projection 00061 float distance_to_plane = pp.dot(model_coefficients); 00062 00063 00064 //TODO: Why doesn't getVector4Map work here? 00065 //Eigen::Vector4f q_e = q.getVector4fMap (); 00066 //q_e = pp - model_coefficients * distance_to_plane; 00067 00068 Eigen::Vector4f q_e = pp - distance_to_plane * model_coefficients; 00069 q.x = q_e[0]; 00070 q.y = q_e[1]; 00071 q.z = q_e[2]; 00072 } 00073 00074 /** \brief Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0 00075 * \param p a point 00076 * \param a the normalized <i>a</i> coefficient of a plane 00077 * \param b the normalized <i>b</i> coefficient of a plane 00078 * \param c the normalized <i>c</i> coefficient of a plane 00079 * \param d the normalized <i>d</i> coefficient of a plane 00080 * \ingroup sample_consensus 00081 */ 00082 template <typename Point> inline double 00083 pointToPlaneDistanceSigned (const Point &p, double a, double b, double c, double d) 00084 { 00085 return (a * p.x + b * p.y + c * p.z + d); 00086 } 00087 00088 /** \brief Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0 00089 * \param p a point 00090 * \param plane_coefficients the normalized coefficients (a, b, c, d) of a plane 00091 * \ingroup sample_consensus 00092 */ 00093 template <typename Point> inline double 00094 pointToPlaneDistanceSigned (const Point &p, const Eigen::Vector4f &plane_coefficients) 00095 { 00096 return ( plane_coefficients[0] * p.x + plane_coefficients[1] * p.y + plane_coefficients[2] * p.z + plane_coefficients[3] ); 00097 } 00098 00099 /** \brief Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0 00100 * \param p a point 00101 * \param a the normalized <i>a</i> coefficient of a plane 00102 * \param b the normalized <i>b</i> coefficient of a plane 00103 * \param c the normalized <i>c</i> coefficient of a plane 00104 * \param d the normalized <i>d</i> coefficient of a plane 00105 * \ingroup sample_consensus 00106 */ 00107 template <typename Point> inline double 00108 pointToPlaneDistance (const Point &p, double a, double b, double c, double d) 00109 { 00110 return (fabs (pointToPlaneDistanceSigned (p, a, b, c, d)) ); 00111 } 00112 00113 /** \brief Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0 00114 * \param p a point 00115 * \param plane_coefficients the normalized coefficients (a, b, c, d) of a plane 00116 * \ingroup sample_consensus 00117 */ 00118 template <typename Point> inline double 00119 pointToPlaneDistance (const Point &p, const Eigen::Vector4f &plane_coefficients) 00120 { 00121 return ( fabs (pointToPlaneDistanceSigned (p, plane_coefficients)) ); 00122 } 00123 00124 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00125 /** \brief SampleConsensusModelPlane defines a model for 3D plane segmentation. 00126 * The model coefficients are defined as: 00127 * - \b a : the X coordinate of the plane's normal (normalized) 00128 * - \b b : the Y coordinate of the plane's normal (normalized) 00129 * - \b c : the Z coordinate of the plane's normal (normalized) 00130 * - \b d : the fourth <a href="http://mathworld.wolfram.com/HessianNormalForm.html">Hessian component</a> of the plane's equation 00131 * 00132 * \author Radu B. Rusu 00133 * \ingroup sample_consensus 00134 */ 00135 template <typename PointT> 00136 class SampleConsensusModelPlane : public SampleConsensusModel<PointT> 00137 { 00138 public: 00139 using SampleConsensusModel<PointT>::input_; 00140 using SampleConsensusModel<PointT>::indices_; 00141 using SampleConsensusModel<PointT>::error_sqr_dists_; 00142 00143 typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud; 00144 typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr; 00145 typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr; 00146 00147 typedef boost::shared_ptr<SampleConsensusModelPlane> Ptr; 00148 00149 /** \brief Constructor for base SampleConsensusModelPlane. 00150 * \param[in] cloud the input point cloud dataset 00151 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00152 */ 00153 SampleConsensusModelPlane (const PointCloudConstPtr &cloud, bool random = false) 00154 : SampleConsensusModel<PointT> (cloud, random) {}; 00155 00156 /** \brief Constructor for base SampleConsensusModelPlane. 00157 * \param[in] cloud the input point cloud dataset 00158 * \param[in] indices a vector of point indices to be used from \a cloud 00159 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) 00160 */ 00161 SampleConsensusModelPlane (const PointCloudConstPtr &cloud, 00162 const std::vector<int> &indices, 00163 bool random = false) 00164 : SampleConsensusModel<PointT> (cloud, indices, random) {}; 00165 00166 /** \brief Empty destructor */ 00167 virtual ~SampleConsensusModelPlane () {} 00168 00169 /** \brief Check whether the given index samples can form a valid plane model, compute the model coefficients from 00170 * these samples and store them internally in model_coefficients_. The plane coefficients are: 00171 * a, b, c, d (ax+by+cz+d=0) 00172 * \param[in] samples the point indices found as possible good candidates for creating a valid model 00173 * \param[out] model_coefficients the resultant model coefficients 00174 */ 00175 bool 00176 computeModelCoefficients (const std::vector<int> &samples, 00177 Eigen::VectorXf &model_coefficients); 00178 00179 /** \brief Compute all distances from the cloud data to a given plane model. 00180 * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to 00181 * \param[out] distances the resultant estimated distances 00182 */ 00183 void 00184 getDistancesToModel (const Eigen::VectorXf &model_coefficients, 00185 std::vector<double> &distances); 00186 00187 /** \brief Select all the points which respect the given model coefficients as inliers. 00188 * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to 00189 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00190 * \param[out] inliers the resultant model inliers 00191 */ 00192 void 00193 selectWithinDistance (const Eigen::VectorXf &model_coefficients, 00194 const double threshold, 00195 std::vector<int> &inliers); 00196 00197 /** \brief Count all the points which respect the given model coefficients as inliers. 00198 * 00199 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to 00200 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers 00201 * \return the resultant number of inliers 00202 */ 00203 virtual int 00204 countWithinDistance (const Eigen::VectorXf &model_coefficients, 00205 const double threshold); 00206 00207 /** \brief Recompute the plane coefficients using the given inlier set and return them to the user. 00208 * @note: these are the coefficients of the plane model after refinement (eg. after SVD) 00209 * \param[in] inliers the data inliers found as supporting the model 00210 * \param[in] model_coefficients the initial guess for the model coefficients 00211 * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization 00212 */ 00213 void 00214 optimizeModelCoefficients (const std::vector<int> &inliers, 00215 const Eigen::VectorXf &model_coefficients, 00216 Eigen::VectorXf &optimized_coefficients); 00217 00218 /** \brief Create a new point cloud with inliers projected onto the plane model. 00219 * \param[in] inliers the data inliers that we want to project on the plane model 00220 * \param[in] model_coefficients the *normalized* coefficients of a plane model 00221 * \param[out] projected_points the resultant projected points 00222 * \param[in] copy_data_fields set to true if we need to copy the other data fields 00223 */ 00224 void 00225 projectPoints (const std::vector<int> &inliers, 00226 const Eigen::VectorXf &model_coefficients, 00227 PointCloud &projected_points, 00228 bool copy_data_fields = true); 00229 00230 /** \brief Verify whether a subset of indices verifies the given plane model coefficients. 00231 * \param[in] indices the data indices that need to be tested against the plane model 00232 * \param[in] model_coefficients the plane model coefficients 00233 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers 00234 */ 00235 bool 00236 doSamplesVerifyModel (const std::set<int> &indices, 00237 const Eigen::VectorXf &model_coefficients, 00238 const double threshold); 00239 00240 /** \brief Return an unique id for this model (SACMODEL_PLANE). */ 00241 inline pcl::SacModel 00242 getModelType () const { return (SACMODEL_PLANE); } 00243 00244 protected: 00245 /** \brief Check whether a model is valid given the user constraints. 00246 * \param[in] model_coefficients the set of model coefficients 00247 */ 00248 inline bool 00249 isModelValid (const Eigen::VectorXf &model_coefficients) 00250 { 00251 // Needs a valid model coefficients 00252 if (model_coefficients.size () != 4) 00253 { 00254 PCL_ERROR ("[pcl::SampleConsensusModelPlane::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00255 return (false); 00256 } 00257 return (true); 00258 } 00259 00260 private: 00261 /** \brief Check if a sample of indices results in a good sample of points 00262 * indices. 00263 * \param[in] samples the resultant index samples 00264 */ 00265 virtual bool 00266 isSampleGood (const std::vector<int> &samples) const; 00267 }; 00268 } 00269 00270 #ifdef PCL_NO_PRECOMPILE 00271 #include <pcl/sample_consensus/impl/sac_model_plane.hpp> 00272 #endif 00273 00274 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_PLANE_H_