Point Cloud Library (PCL)  1.7.1
sac_model_registration_2d.h
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 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_2D_H_
40 #define PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_2D_H_
41 
42 #include <pcl/sample_consensus/sac_model_registration.h>
43 
44 namespace pcl
45 {
46  /** \brief SampleConsensusModelRegistration2D defines a model for Point-To-Point registration outlier rejection using distances between 2D pixels
47  * \author Radu B. Rusu
48  * \ingroup sample_consensus
49  */
50  template <typename PointT>
52  {
53  public:
62 
66 
67  typedef boost::shared_ptr<SampleConsensusModelRegistration2D> Ptr;
68  typedef boost::shared_ptr<const SampleConsensusModelRegistration2D> ConstPtr;
69 
70  /** \brief Constructor for base SampleConsensusModelRegistration2D.
71  * \param[in] cloud the input point cloud dataset
72  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
73  */
75  bool random = false)
76  : pcl::SampleConsensusModelRegistration<PointT> (cloud, random)
77  , projection_matrix_ (Eigen::Matrix3f::Identity ())
78  {
79  // Call our own setInputCloud
80  setInputCloud (cloud);
81  }
82 
83  /** \brief Constructor for base SampleConsensusModelRegistration2D.
84  * \param[in] cloud the input point cloud dataset
85  * \param[in] indices a vector of point indices to be used from \a cloud
86  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
87  */
89  const std::vector<int> &indices,
90  bool random = false)
91  : pcl::SampleConsensusModelRegistration<PointT> (cloud, indices, random)
92  , projection_matrix_ (Eigen::Matrix3f::Identity ())
93  {
95  computeSampleDistanceThreshold (cloud, indices);
96  }
97 
98  /** \brief Empty destructor */
100 
101  /** \brief Compute all distances from the transformed points to their correspondences
102  * \param[in] model_coefficients the 4x4 transformation matrix
103  * \param[out] distances the resultant estimated distances
104  */
105  void
106  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
107  std::vector<double> &distances);
108 
109  /** \brief Select all the points which respect the given model coefficients as inliers.
110  * \param[in] model_coefficients the 4x4 transformation matrix
111  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
112  * \param[out] inliers the resultant model inliers
113  */
114  void
115  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
116  const double threshold,
117  std::vector<int> &inliers);
118 
119  /** \brief Count all the points which respect the given model coefficients as inliers.
120  *
121  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
122  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
123  * \return the resultant number of inliers
124  */
125  virtual int
126  countWithinDistance (const Eigen::VectorXf &model_coefficients,
127  const double threshold);
128 
129  /** \brief Set the camera projection matrix.
130  * \param[in] projection_matrix the camera projection matrix
131  */
132  inline void
133  setProjectionMatrix (const Eigen::Matrix3f &projection_matrix)
134  { projection_matrix_ = projection_matrix; }
135 
136  /** \brief Get the camera projection matrix. */
137  inline Eigen::Matrix3f
139  { return (projection_matrix_); }
140 
141  protected:
142  /** \brief Check if a sample of indices results in a good sample of points
143  * indices.
144  * \param[in] samples the resultant index samples
145  */
146  bool
147  isSampleGood (const std::vector<int> &samples) const;
148 
149  /** \brief Computes an "optimal" sample distance threshold based on the
150  * principal directions of the input cloud.
151  * \param[in] cloud the const boost shared pointer to a PointCloud message
152  */
153  inline void
155  {
156  //// Compute the principal directions via PCA
157  //Eigen::Vector4f xyz_centroid;
158  //Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero ();
159 
160  //computeMeanAndCovarianceMatrix (*cloud, covariance_matrix, xyz_centroid);
161 
162  //// Check if the covariance matrix is finite or not.
163  //for (int i = 0; i < 3; ++i)
164  // for (int j = 0; j < 3; ++j)
165  // if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
166  // PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
167 
168  //Eigen::Vector3f eigen_values;
169  //pcl::eigen33 (covariance_matrix, eigen_values);
170 
171  //// Compute the distance threshold for sample selection
172  //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
173  //sample_dist_thresh_ *= sample_dist_thresh_;
174  //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
175  }
176 
177  /** \brief Computes an "optimal" sample distance threshold based on the
178  * principal directions of the input cloud.
179  * \param[in] cloud the const boost shared pointer to a PointCloud message
180  */
181  inline void
183  const std::vector<int> &indices)
184  {
185  //// Compute the principal directions via PCA
186  //Eigen::Vector4f xyz_centroid;
187  //Eigen::Matrix3f covariance_matrix;
188  //computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid);
189 
190  //// Check if the covariance matrix is finite or not.
191  //for (int i = 0; i < 3; ++i)
192  // for (int j = 0; j < 3; ++j)
193  // if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
194  // PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
195 
196  //Eigen::Vector3f eigen_values;
197  //pcl::eigen33 (covariance_matrix, eigen_values);
198 
199  //// Compute the distance threshold for sample selection
200  //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
201  //sample_dist_thresh_ *= sample_dist_thresh_;
202  //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
203  }
204 
205  private:
206  /** \brief Camera projection matrix. */
207  Eigen::Matrix3f projection_matrix_;
208 
209  public:
210  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
211  };
212 }
213 
214 #include <pcl/sample_consensus/impl/sac_model_registration_2d.hpp>
215 
216 #endif // PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_2D_H_
217