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