sac_model_registration_2d.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2012-, Open Perception, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  *
00037  */
00038 
00039 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_2D_H_
00040 #define PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_2D_H_
00041 
00042 #include <pcl/sample_consensus/sac_model_registration.h>
00043 
00044 namespace pcl
00045 {
00050   template <typename PointT>
00051   class SampleConsensusModelRegistration2D : public pcl::SampleConsensusModelRegistration<PointT>
00052   {
00053     public:
00054       using pcl::SampleConsensusModelRegistration<PointT>::input_;
00055       using pcl::SampleConsensusModelRegistration<PointT>::target_;
00056       using pcl::SampleConsensusModelRegistration<PointT>::indices_;
00057       using pcl::SampleConsensusModelRegistration<PointT>::indices_tgt_;
00058       using pcl::SampleConsensusModelRegistration<PointT>::error_sqr_dists_;
00059       using pcl::SampleConsensusModelRegistration<PointT>::correspondences_;
00060       using pcl::SampleConsensusModelRegistration<PointT>::sample_dist_thresh_;
00061       using pcl::SampleConsensusModelRegistration<PointT>::computeOriginalIndexMapping;
00062 
00063       typedef typename pcl::SampleConsensusModel<PointT>::PointCloud PointCloud;
00064       typedef typename pcl::SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
00065       typedef typename pcl::SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr;
00066 
00067       typedef boost::shared_ptr<SampleConsensusModelRegistration2D> Ptr;
00068       typedef boost::shared_ptr<const SampleConsensusModelRegistration2D> ConstPtr;
00069 
00074       SampleConsensusModelRegistration2D (const PointCloudConstPtr &cloud,
00075                                           bool random = false) 
00076         : pcl::SampleConsensusModelRegistration<PointT> (cloud, random)
00077         , projection_matrix_ (Eigen::Matrix3f::Identity ())
00078       {
00079         // Call our own setInputCloud
00080         setInputCloud (cloud);
00081       }
00082 
00088       SampleConsensusModelRegistration2D (const PointCloudConstPtr &cloud,
00089                                           const std::vector<int> &indices,
00090                                           bool random = false)
00091         : pcl::SampleConsensusModelRegistration<PointT> (cloud, indices, random)
00092         , projection_matrix_ (Eigen::Matrix3f::Identity ())
00093       {
00094         computeOriginalIndexMapping ();
00095         computeSampleDistanceThreshold (cloud, indices);
00096       }
00097       
00099       virtual ~SampleConsensusModelRegistration2D () {}
00100 
00105       void
00106       getDistancesToModel (const Eigen::VectorXf &model_coefficients,
00107                            std::vector<double> &distances);
00108 
00114       void
00115       selectWithinDistance (const Eigen::VectorXf &model_coefficients,
00116                             const double threshold,
00117                             std::vector<int> &inliers);
00118 
00125       virtual int
00126       countWithinDistance (const Eigen::VectorXf &model_coefficients,
00127                            const double threshold);
00128 
00132       inline void
00133       setProjectionMatrix (const Eigen::Matrix3f &projection_matrix)
00134       { projection_matrix_ = projection_matrix; }
00135 
00137       inline Eigen::Matrix3f
00138       getProjectionMatrix () const
00139       { return (projection_matrix_); }
00140 
00141     protected:
00146       bool
00147       isSampleGood (const std::vector<int> &samples) const;
00148 
00153       inline void
00154       computeSampleDistanceThreshold (const PointCloudConstPtr &cloud)
00155       {
00157         //Eigen::Vector4f xyz_centroid;
00158         //Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero ();
00159 
00160         //computeMeanAndCovarianceMatrix (*cloud, covariance_matrix, xyz_centroid);
00161 
00163         //for (int i = 0; i < 3; ++i)
00164         //  for (int j = 0; j < 3; ++j)
00165         //    if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
00166         //      PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
00167 
00168         //Eigen::Vector3f eigen_values;
00169         //pcl::eigen33 (covariance_matrix, eigen_values);
00170 
00172         //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
00173         //sample_dist_thresh_ *= sample_dist_thresh_;
00174         //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
00175       }
00176 
00181       inline void
00182       computeSampleDistanceThreshold (const PointCloudConstPtr &cloud,
00183                                       const std::vector<int> &indices)
00184       {
00186         //Eigen::Vector4f xyz_centroid;
00187         //Eigen::Matrix3f covariance_matrix;
00188         //computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid);
00189 
00191         //for (int i = 0; i < 3; ++i)
00192         //  for (int j = 0; j < 3; ++j)
00193         //    if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
00194         //      PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
00195 
00196         //Eigen::Vector3f eigen_values;
00197         //pcl::eigen33 (covariance_matrix, eigen_values);
00198 
00200         //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
00201         //sample_dist_thresh_ *= sample_dist_thresh_;
00202         //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
00203       }
00204 
00205     private:
00207       Eigen::Matrix3f projection_matrix_;
00208 
00209     public:
00210       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00211   };
00212 }
00213 
00214 #include <pcl/sample_consensus/impl/sac_model_registration_2d.hpp>
00215 
00216 #endif    // PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_2D_H_
00217 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:32:17