1 /*
2  * Software License Agreement (BSD License)
3  *
4  *  Point Cloud Library (PCL) - www.pointclouds.org
5  *  Copyright (c) 2012-, Open Perception, Inc.
6  *
7  *  All rights reserved.
8  *
9  *  Redistribution and use in source and binary forms, with or without
10  *  modification, are permitted provided that the following conditions
11  *  are met:
12  *
13  *   * Redistributions of source code must retain the above copyright
14  *     notice, this list of conditions and the following disclaimer.
15  *   * Redistributions in binary form must reproduce the above
16  *     copyright notice, this list of conditions and the following
17  *     disclaimer in the documentation and/or other materials provided
18  *     with the distribution.
19  *   * Neither the name of the copyright holder(s) nor the names of its
20  *     contributors may be used to endorse or promote products derived
21  *     from this software without specific prior written permission.
22  *
23  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  *  POSSIBILITY OF SUCH DAMAGE.
35  *
36  *
37  */
38 
39 #pragma once
40 
41 #include <pcl/sample_consensus/sac_model_registration.h>
42 #include <pcl/memory.h>
43 #include <pcl/pcl_macros.h>
44 
45 namespace pcl
46 {
47   /** \brief SampleConsensusModelRegistration2D defines a model for Point-To-Point registration outlier rejection using distances between 2D pixels
48     * \author Radu B. Rusu
49     * \ingroup sample_consensus
50     */
51   template <typename PointT>
52   class SampleConsensusModelRegistration2D : public pcl::SampleConsensusModelRegistration<PointT>
53   {
54     public:
55       using pcl::SampleConsensusModelRegistration<PointT>::model_name_;
56       using pcl::SampleConsensusModelRegistration<PointT>::input_;
57       using pcl::SampleConsensusModelRegistration<PointT>::target_;
58       using pcl::SampleConsensusModelRegistration<PointT>::indices_;
59       using pcl::SampleConsensusModelRegistration<PointT>::indices_tgt_;
60       using pcl::SampleConsensusModelRegistration<PointT>::error_sqr_dists_;
61       using pcl::SampleConsensusModelRegistration<PointT>::correspondences_;
62       using pcl::SampleConsensusModelRegistration<PointT>::sample_dist_thresh_;
63       using pcl::SampleConsensusModelRegistration<PointT>::computeOriginalIndexMapping;
64       using pcl::SampleConsensusModel<PointT>::isModelValid;
65 
66       using PointCloud = typename pcl::SampleConsensusModel<PointT>::PointCloud;
67       using PointCloudPtr = typename pcl::SampleConsensusModel<PointT>::PointCloudPtr;
68       using PointCloudConstPtr = typename pcl::SampleConsensusModel<PointT>::PointCloudConstPtr;
69 
70       using Ptr = shared_ptr<SampleConsensusModelRegistration2D<PointT> >;
71       using ConstPtr = shared_ptr<const SampleConsensusModelRegistration2D<PointT> >;
72 
73       /** \brief Constructor for base SampleConsensusModelRegistration2D.
74         * \param[in] cloud the input point cloud dataset
75         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
76         */
77       SampleConsensusModelRegistration2D (const PointCloudConstPtr &cloud,
78                                           bool random = false)
79         : pcl::SampleConsensusModelRegistration<PointT> (cloud, random)
80         , projection_matrix_ (Eigen::Matrix3f::Identity ())
81       {
82         // Call our own setInputCloud
83         setInputCloud (cloud);
84         model_name_ = "SampleConsensusModelRegistration2D";
85         sample_size_ = 3;
86         model_size_ = 16;
87       }
88 
89       /** \brief Constructor for base SampleConsensusModelRegistration2D.
90         * \param[in] cloud the input point cloud dataset
91         * \param[in] indices a vector of point indices to be used from \a cloud
92         * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
93         */
94       SampleConsensusModelRegistration2D (const PointCloudConstPtr &cloud,
95                                           const Indices &indices,
96                                           bool random = false)
97         : pcl::SampleConsensusModelRegistration<PointT> (cloud, indices, random)
98         , projection_matrix_ (Eigen::Matrix3f::Identity ())
99       {
100         computeOriginalIndexMapping ();
101         computeSampleDistanceThreshold (cloud, indices);
102         model_name_ = "SampleConsensusModelRegistration2D";
103         sample_size_ = 3;
104         model_size_ = 16;
105       }
106 
107       /** \brief Empty destructor */
~SampleConsensusModelRegistration2D()108       virtual ~SampleConsensusModelRegistration2D () {}
109 
110       /** \brief Compute all distances from the transformed points to their correspondences
111         * \param[in] model_coefficients the 4x4 transformation matrix
112         * \param[out] distances the resultant estimated distances
113         */
114       void
115       getDistancesToModel (const Eigen::VectorXf &model_coefficients,
116                            std::vector<double> &distances) const;
117 
118       /** \brief Select all the points which respect the given model coefficients as inliers.
119         * \param[in] model_coefficients the 4x4 transformation matrix
120         * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
121         * \param[out] inliers the resultant model inliers
122         */
123       void
124       selectWithinDistance (const Eigen::VectorXf &model_coefficients,
125                             const double threshold,
126                             Indices &inliers);
127 
128       /** \brief Count all the points which respect the given model coefficients as inliers.
129         *
130         * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
131         * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
132         * \return the resultant number of inliers
133         */
134       virtual std::size_t
135       countWithinDistance (const Eigen::VectorXf &model_coefficients,
136                            const double threshold) const;
137 
138       /** \brief Set the camera projection matrix.
139         * \param[in] projection_matrix the camera projection matrix
140         */
141       inline void
setProjectionMatrix(const Eigen::Matrix3f & projection_matrix)142       setProjectionMatrix (const Eigen::Matrix3f &projection_matrix)
143       { projection_matrix_ = projection_matrix; }
144 
145       /** \brief Get the camera projection matrix. */
146       inline Eigen::Matrix3f
getProjectionMatrix()147       getProjectionMatrix () const
148       { return (projection_matrix_); }
149 
150     protected:
151       using SampleConsensusModel<PointT>::sample_size_;
152       using SampleConsensusModel<PointT>::model_size_;
153 
154       /** \brief Check if a sample of indices results in a good sample of points
155         * indices.
156         * \param[in] samples the resultant index samples
157         */
158       bool
159       isSampleGood (const Indices &samples) const;
160 
161       /** \brief Computes an "optimal" sample distance threshold based on the
162         * principal directions of the input cloud.
163         */
164       inline void
computeSampleDistanceThreshold(const PointCloudConstPtr &)165       computeSampleDistanceThreshold (const PointCloudConstPtr&)
166       {
167         //// Compute the principal directions via PCA
168         //Eigen::Vector4f xyz_centroid;
169         //Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero ();
170 
171         //computeMeanAndCovarianceMatrix (*cloud, covariance_matrix, xyz_centroid);
172 
173         //// Check if the covariance matrix is finite or not.
174         //for (int i = 0; i < 3; ++i)
175         //  for (int j = 0; j < 3; ++j)
176         //    if (!std::isfinite (covariance_matrix.coeffRef (i, j)))
177         //      PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
178 
179         //Eigen::Vector3f eigen_values;
180         //pcl::eigen33 (covariance_matrix, eigen_values);
181 
182         //// Compute the distance threshold for sample selection
183         //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
184         //sample_dist_thresh_ *= sample_dist_thresh_;
185         //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
186       }
187 
188       /** \brief Computes an "optimal" sample distance threshold based on the
189         * principal directions of the input cloud.
190         */
191       inline void
computeSampleDistanceThreshold(const PointCloudConstPtr &,const Indices &)192       computeSampleDistanceThreshold (const PointCloudConstPtr&,
193                                       const Indices&)
194       {
195         //// Compute the principal directions via PCA
196         //Eigen::Vector4f xyz_centroid;
197         //Eigen::Matrix3f covariance_matrix;
198         //computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid);
199 
200         //// Check if the covariance matrix is finite or not.
201         //for (int i = 0; i < 3; ++i)
202         //  for (int j = 0; j < 3; ++j)
203         //    if (!std::isfinite (covariance_matrix.coeffRef (i, j)))
204         //      PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
205 
206         //Eigen::Vector3f eigen_values;
207         //pcl::eigen33 (covariance_matrix, eigen_values);
208 
209         //// Compute the distance threshold for sample selection
210         //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
211         //sample_dist_thresh_ *= sample_dist_thresh_;
212         //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
213       }
214 
215     private:
216       /** \brief Camera projection matrix. */
217       Eigen::Matrix3f projection_matrix_;
218 
219     public:
220       PCL_MAKE_ALIGNED_OPERATOR_NEW
221   };
222 }
223 
224 #include <pcl/sample_consensus/impl/sac_model_registration_2d.hpp>
225