Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_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_PARALLELLINE_H_
00042 #define PCL_SAMPLE_CONSENSUS_MODEL_PARALLELLINE_H_
00043 
00044 #include <pcl/sample_consensus/sac_model_line.h>
00045 #include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
00046 
00047 namespace pcl
00048 {
00049   /** \brief SampleConsensusModelParallelLine defines a model for 3D line segmentation using additional angular
00050     * constraints.
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 SampleConsensusModelParallelLine : public SampleConsensusModelLine<PointT>
00064   {
00065     public:
00066       typedef typename SampleConsensusModelLine<PointT>::PointCloud PointCloud;
00067       typedef typename SampleConsensusModelLine<PointT>::PointCloudPtr PointCloudPtr;
00068       typedef typename SampleConsensusModelLine<PointT>::PointCloudConstPtr PointCloudConstPtr;
00069 
00070       typedef boost::shared_ptr<SampleConsensusModelParallelLine> Ptr;
00071 
00072       /** \brief Constructor for base SampleConsensusModelParallelLine.
00073         * \param[in] cloud the input point cloud dataset
00074         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00075         */
00076       SampleConsensusModelParallelLine (const PointCloudConstPtr &cloud, 
00077                                         bool random = false) 
00078         : SampleConsensusModelLine<PointT> (cloud, random)
00079         , axis_ (Eigen::Vector3f::Zero ())
00080         , eps_angle_ (0.0)
00081       {
00082       }
00083 
00084       /** \brief Constructor for base SampleConsensusModelParallelLine.
00085         * \param[in] cloud the input point cloud dataset
00086         * \param[in] indices a vector of point indices to be used from \a cloud
00087         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00088         */
00089       SampleConsensusModelParallelLine (const PointCloudConstPtr &cloud, 
00090                                         const std::vector<int> &indices,
00091                                         bool random = false) 
00092         : SampleConsensusModelLine<PointT> (cloud, indices, random)
00093         , axis_ (Eigen::Vector3f::Zero ())
00094         , eps_angle_ (0.0)
00095       {
00096       }
00097       
00098       /** \brief Empty destructor */
00099       virtual ~SampleConsensusModelParallelLine () {}
00100 
00101       /** \brief Set the axis along which we need to search for a plane perpendicular to.
00102         * \param[in] ax the axis along which we need to search for a plane perpendicular to
00103         */
00104       inline void 
00105       setAxis (const Eigen::Vector3f &ax) { axis_ = ax; axis_.normalize (); }
00106 
00107       /** \brief Get the axis along which we need to search for a plane perpendicular to. */
00108       inline Eigen::Vector3f 
00109       getAxis ()  { return (axis_); }
00110 
00111       /** \brief Set the angle epsilon (delta) threshold.
00112         * \param[in] ea the maximum allowed difference between the plane normal and the given axis.
00113         */
00114       inline void 
00115       setEpsAngle (const double ea) { eps_angle_ = ea; }
00116 
00117       /** \brief Get the angle epsilon (delta) threshold. */
00118       inline double getEpsAngle () { return (eps_angle_); }
00119 
00120       /** \brief Select all the points which respect the given model coefficients as inliers.
00121         * \param[in] model_coefficients the coefficients of a line model that we need to compute distances to
00122         * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
00123         * \param[out] inliers the resultant model inliers
00124         */
00125       void 
00126       selectWithinDistance (const Eigen::VectorXf &model_coefficients, 
00127                             const double threshold, 
00128                             std::vector<int> &inliers);
00129 
00130       /** \brief Count all the points which respect the given model coefficients as inliers. 
00131         * 
00132         * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
00133         * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
00134         * \return the resultant number of inliers
00135         */
00136       virtual int
00137       countWithinDistance (const Eigen::VectorXf &model_coefficients, 
00138                            const double threshold);
00139 
00140       /** \brief Compute all squared distances from the cloud data to a given line model.
00141         * \param[in] model_coefficients the coefficients of a line model that we need to compute distances to
00142         * \param[out] distances the resultant estimated squared distances
00143         */
00144       void 
00145       getDistancesToModel (const Eigen::VectorXf &model_coefficients, 
00146                            std::vector<double> &distances);
00147 
00148       /** \brief Return an unique id for this model (SACMODEL_PARALLEL_LINE). */
00149       inline pcl::SacModel 
00150       getModelType () const { return (SACMODEL_PARALLEL_LINE); }
00151 
00152     protected:
00153       /** \brief Check whether a model is valid given the user constraints.
00154         * \param[in] model_coefficients the set of model coefficients
00155         */
00156       bool 
00157       isModelValid (const Eigen::VectorXf &model_coefficients);
00158 
00159     protected:
00160       /** \brief The axis along which we need to search for a plane perpendicular to. */
00161       Eigen::Vector3f axis_;
00162 
00163       /** \brief The maximum allowed difference between the plane normal and the given axis. */
00164       double eps_angle_;
00165   };
00166 }
00167 
00168 #ifdef PCL_NO_PRECOMPILE
00169 #include <pcl/sample_consensus/impl/sac_model_parallel_line.hpp>
00170 #endif
00171 
00172 #endif  //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_PARALLELLINE_H_