Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/sample_consensus/include/pcl/sample_consensus/sac_model_registration_2d.h
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 {
00046   /** \brief SampleConsensusModelRegistration2D defines a model for Point-To-Point registration outlier rejection using distances between 2D pixels
00047     * \author Radu B. Rusu
00048     * \ingroup sample_consensus
00049     */
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 
00070       /** \brief Constructor for base SampleConsensusModelRegistration2D.
00071         * \param[in] cloud the input point cloud dataset
00072         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
00073         */
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 
00083       /** \brief Constructor for base SampleConsensusModelRegistration2D.
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       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       
00098       /** \brief Empty destructor */
00099       virtual ~SampleConsensusModelRegistration2D () {}
00100 
00101       /** \brief Compute all distances from the transformed points to their correspondences
00102         * \param[in] model_coefficients the 4x4 transformation matrix
00103         * \param[out] distances the resultant estimated distances
00104         */
00105       void
00106       getDistancesToModel (const Eigen::VectorXf &model_coefficients,
00107                            std::vector<double> &distances);
00108 
00109       /** \brief Select all the points which respect the given model coefficients as inliers.
00110         * \param[in] model_coefficients the 4x4 transformation matrix
00111         * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
00112         * \param[out] inliers the resultant model inliers
00113         */
00114       void
00115       selectWithinDistance (const Eigen::VectorXf &model_coefficients,
00116                             const double threshold,
00117                             std::vector<int> &inliers);
00118 
00119       /** \brief Count all the points which respect the given model coefficients as inliers.
00120         *
00121         * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
00122         * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
00123         * \return the resultant number of inliers
00124         */
00125       virtual int
00126       countWithinDistance (const Eigen::VectorXf &model_coefficients,
00127                            const double threshold);
00128 
00129       /** \brief Set the camera projection matrix. 
00130         * \param[in] projection_matrix the camera projection matrix 
00131         */
00132       inline void
00133       setProjectionMatrix (const Eigen::Matrix3f &projection_matrix)
00134       { projection_matrix_ = projection_matrix; }
00135 
00136       /** \brief Get the camera projection matrix. */
00137       inline Eigen::Matrix3f
00138       getProjectionMatrix () const
00139       { return (projection_matrix_); }
00140 
00141     protected:
00142       /** \brief Check if a sample of indices results in a good sample of points
00143         * indices.
00144         * \param[in] samples the resultant index samples
00145         */
00146       bool
00147       isSampleGood (const std::vector<int> &samples) const;
00148 
00149       /** \brief Computes an "optimal" sample distance threshold based on the
00150         * principal directions of the input cloud.
00151         * \param[in] cloud the const boost shared pointer to a PointCloud message
00152         */
00153       inline void
00154       computeSampleDistanceThreshold (const PointCloudConstPtr &cloud)
00155       {
00156         //// Compute the principal directions via PCA
00157         //Eigen::Vector4f xyz_centroid;
00158         //Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero ();
00159 
00160         //computeMeanAndCovarianceMatrix (*cloud, covariance_matrix, xyz_centroid);
00161 
00162         //// Check if the covariance matrix is finite or not.
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 
00171         //// Compute the distance threshold for sample selection
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 
00177       /** \brief Computes an "optimal" sample distance threshold based on the
00178         * principal directions of the input cloud.
00179         * \param[in] cloud the const boost shared pointer to a PointCloud message
00180         */
00181       inline void
00182       computeSampleDistanceThreshold (const PointCloudConstPtr &cloud,
00183                                       const std::vector<int> &indices)
00184       {
00185         //// Compute the principal directions via PCA
00186         //Eigen::Vector4f xyz_centroid;
00187         //Eigen::Matrix3f covariance_matrix;
00188         //computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid);
00189 
00190         //// Check if the covariance matrix is finite or not.
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 
00199         //// Compute the distance threshold for sample selection
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:
00206       /** \brief Camera projection matrix. */
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