Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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#pragma once
40
41#include <pcl/sample_consensus/sac_model_registration.h>
42#include <pcl/memory.h>
43#include <pcl/pcl_macros.h>
44
45namespace 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>
53 {
54 public:
65
69
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 */
78 bool random = false)
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 */
95 const Indices &indices,
96 bool random = false)
98 , projection_matrix_ (Eigen::Matrix3f::Identity ())
99 {
101 computeSampleDistanceThreshold (cloud, indices);
102 model_name_ = "SampleConsensusModelRegistration2D";
103 sample_size_ = 3;
104 model_size_ = 16;
105 }
106
107 /** \brief Empty destructor */
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,
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
142 setProjectionMatrix (const Eigen::Matrix3f &projection_matrix)
143 { projection_matrix_ = projection_matrix; }
144
145 /** \brief Get the camera projection matrix. */
146 inline Eigen::Matrix3f
148 { return (projection_matrix_); }
149
150 protected:
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
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
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:
221 };
222}
223
224#include <pcl/sample_consensus/impl/sac_model_registration_2d.hpp>
Iterator class for point clouds with or without given indices.
PointCloud represents the base class in PCL for storing collections of 3D points.
SampleConsensusModel represents the base model class.
Definition sac_model.h:70
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition sac_model.h:588
typename PointCloud::ConstPtr PointCloudConstPtr
Definition sac_model.h:73
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition sac_model.h:556
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition sac_model.h:553
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
Definition sac_model.h:527
std::string model_name_
The model name.
Definition sac_model.h:550
unsigned int model_size_
The number of coefficients in the model.
Definition sac_model.h:591
typename PointCloud::Ptr PointCloudPtr
Definition sac_model.h:74
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
Definition sac_model.h:585
SampleConsensusModelRegistration2D defines a model for Point-To-Point registration outlier rejection ...
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)
Select all the points which respect the given model coefficients as inliers.
typename pcl::SampleConsensusModel< PointT >::PointCloud PointCloud
SampleConsensusModelRegistration2D(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelRegistration2D.
typename pcl::SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
typename pcl::SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
void setProjectionMatrix(const Eigen::Matrix3f &projection_matrix)
Set the camera projection matrix.
Eigen::Matrix3f getProjectionMatrix() const
Get the camera projection matrix.
virtual std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
SampleConsensusModelRegistration2D(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelRegistration2D.
void computeSampleDistanceThreshold(const PointCloudConstPtr &)
Computes an "optimal" sample distance threshold based on the principal directions of the input cloud.
void computeSampleDistanceThreshold(const PointCloudConstPtr &, const Indices &)
Computes an "optimal" sample distance threshold based on the principal directions of the input cloud.
bool isSampleGood(const Indices &samples) const
Check if a sample of indices results in a good sample of points indices.
virtual ~SampleConsensusModelRegistration2D()
Empty destructor.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the transformed points to their correspondences.
SampleConsensusModelRegistration defines a model for Point-To-Point registration outlier rejection.
std::map< index_t, index_t > correspondences_
Given the index in the original point cloud, give the matching original index in the target cloud.
IndicesPtr indices_tgt_
A pointer to the vector of target point indices to use.
PointCloudConstPtr target_
A boost shared pointer to the target point cloud data array.
double sample_dist_thresh_
Internal distance threshold used for the sample selection step.
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
void computeOriginalIndexMapping()
Compute mappings between original indices of the input_/target_ clouds.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:63
Defines functions, macros and traits for allocating and using memory.
Definition bfgs.h:10
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, and the RGB color.