Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h
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_