Point Cloud Library (PCL)  1.8.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:
64 
68 
69  typedef boost::shared_ptr<SampleConsensusModelRegistration2D> Ptr;
70  typedef boost::shared_ptr<const SampleConsensusModelRegistration2D> ConstPtr;
71 
72  /** \brief Constructor for base SampleConsensusModelRegistration2D.
73  * \param[in] cloud the input point cloud dataset
74  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
75  */
77  bool random = false)
78  : pcl::SampleConsensusModelRegistration<PointT> (cloud, random)
79  , projection_matrix_ (Eigen::Matrix3f::Identity ())
80  {
81  // Call our own setInputCloud
82  setInputCloud (cloud);
83  model_name_ = "SampleConsensusModelRegistration2D";
84  sample_size_ = 3;
85  model_size_ = 16;
86  }
87 
88  /** \brief Constructor for base SampleConsensusModelRegistration2D.
89  * \param[in] cloud the input point cloud dataset
90  * \param[in] indices a vector of point indices to be used from \a cloud
91  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
92  */
94  const std::vector<int> &indices,
95  bool random = false)
96  : pcl::SampleConsensusModelRegistration<PointT> (cloud, indices, random)
97  , projection_matrix_ (Eigen::Matrix3f::Identity ())
98  {
100  computeSampleDistanceThreshold (cloud, indices);
101  model_name_ = "SampleConsensusModelRegistration2D";
102  sample_size_ = 3;
103  model_size_ = 16;
104  }
105 
106  /** \brief Empty destructor */
108 
109  /** \brief Compute all distances from the transformed points to their correspondences
110  * \param[in] model_coefficients the 4x4 transformation matrix
111  * \param[out] distances the resultant estimated distances
112  */
113  void
114  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
115  std::vector<double> &distances);
116 
117  /** \brief Select all the points which respect the given model coefficients as inliers.
118  * \param[in] model_coefficients the 4x4 transformation matrix
119  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
120  * \param[out] inliers the resultant model inliers
121  */
122  void
123  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
124  const double threshold,
125  std::vector<int> &inliers);
126 
127  /** \brief Count all the points which respect the given model coefficients as inliers.
128  *
129  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
130  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
131  * \return the resultant number of inliers
132  */
133  virtual int
134  countWithinDistance (const Eigen::VectorXf &model_coefficients,
135  const double threshold);
136 
137  /** \brief Set the camera projection matrix.
138  * \param[in] projection_matrix the camera projection matrix
139  */
140  inline void
141  setProjectionMatrix (const Eigen::Matrix3f &projection_matrix)
142  { projection_matrix_ = projection_matrix; }
143 
144  /** \brief Get the camera projection matrix. */
145  inline Eigen::Matrix3f
147  { return (projection_matrix_); }
148 
149  protected:
152 
153  /** \brief Check if a sample of indices results in a good sample of points
154  * indices.
155  * \param[in] samples the resultant index samples
156  */
157  bool
158  isSampleGood (const std::vector<int> &samples) const;
159 
160  /** \brief Computes an "optimal" sample distance threshold based on the
161  * principal directions of the input cloud.
162  */
163  inline void
165  {
166  //// Compute the principal directions via PCA
167  //Eigen::Vector4f xyz_centroid;
168  //Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero ();
169 
170  //computeMeanAndCovarianceMatrix (*cloud, covariance_matrix, xyz_centroid);
171 
172  //// Check if the covariance matrix is finite or not.
173  //for (int i = 0; i < 3; ++i)
174  // for (int j = 0; j < 3; ++j)
175  // if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
176  // PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
177 
178  //Eigen::Vector3f eigen_values;
179  //pcl::eigen33 (covariance_matrix, eigen_values);
180 
181  //// Compute the distance threshold for sample selection
182  //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
183  //sample_dist_thresh_ *= sample_dist_thresh_;
184  //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
185  }
186 
187  /** \brief Computes an "optimal" sample distance threshold based on the
188  * principal directions of the input cloud.
189  */
190  inline void
192  const std::vector<int>&)
193  {
194  //// Compute the principal directions via PCA
195  //Eigen::Vector4f xyz_centroid;
196  //Eigen::Matrix3f covariance_matrix;
197  //computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid);
198 
199  //// Check if the covariance matrix is finite or not.
200  //for (int i = 0; i < 3; ++i)
201  // for (int j = 0; j < 3; ++j)
202  // if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
203  // PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
204 
205  //Eigen::Vector3f eigen_values;
206  //pcl::eigen33 (covariance_matrix, eigen_values);
207 
208  //// Compute the distance threshold for sample selection
209  //sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
210  //sample_dist_thresh_ *= sample_dist_thresh_;
211  //PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
212  }
213 
214  private:
215  /** \brief Camera projection matrix. */
216  Eigen::Matrix3f projection_matrix_;
217 
218  public:
219  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
220  };
221 }
222 
223 #include <pcl/sample_consensus/impl/sac_model_registration_2d.hpp>
224 
225 #endif // PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_2D_H_
226 
SampleConsensusModelRegistration defines a model for Point-To-Point registration outlier rejection...
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the transformed points to their correspondences.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers)
Select all the points which respect the given model coefficients as inliers.
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:575
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
Eigen::Matrix3f getProjectionMatrix() const
Get the camera projection matrix.
pcl::SampleConsensusModel< PointT >::PointCloud PointCloud
Definition: bfgs.h:10
boost::shared_ptr< const SampleConsensusModelRegistration2D > ConstPtr
SampleConsensusModelRegistration2D(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelRegistration2D.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
virtual ~SampleConsensusModelRegistration2D()
Empty destructor.
void computeSampleDistanceThreshold(const PointCloudConstPtr &)
Computes an "optimal" sample distance threshold based on the principal directions of the input cloud...
SampleConsensusModel represents the base model class.
Definition: sac_model.h:66
std::string model_name_
The model name.
Definition: sac_model.h:534
pcl::PointCloud< PointT >::Ptr PointCloudPtr
Definition: sac_model.h:71
SampleConsensusModelRegistration2D defines a model for Point-To-Point registration outlier rejection ...
void computeOriginalIndexMapping()
Compute mappings between original indices of the input_/target_ clouds.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
Definition: sac_model.h:70
pcl::SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
SampleConsensusModelRegistration2D(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelRegistration2D.
void computeSampleDistanceThreshold(const PointCloudConstPtr &, const std::vector< int > &)
Computes an "optimal" sample distance threshold based on the principal directions of the input cloud...
A point structure representing Euclidean xyz coordinates, and the RGB color.
void setProjectionMatrix(const Eigen::Matrix3f &projection_matrix)
Set the camera projection matrix.
pcl::SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:572
boost::shared_ptr< SampleConsensusModelRegistration2D > Ptr