41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_
44 #include <pcl/sample_consensus/sac_model_plane.h>
45 #include <pcl/common/centroid.h>
46 #include <pcl/common/eigen.h>
47 #include <pcl/common/concatenate.h>
48 #include <pcl/point_types.h>
51 template <
typename Po
intT>
bool
62 Eigen::Array4f dy1dy2 = (p1-p0) / (p2-p0);
64 return ( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) );
68 template <
typename Po
intT>
bool
70 const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
const
73 if (samples.size () != sample_size_)
75 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
84 Eigen::Array4f p1p0 = p1 - p0;
86 Eigen::Array4f p2p0 = p2 - p0;
89 Eigen::Array4f dy1dy2 = p1p0 / p2p0;
90 if ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) )
95 model_coefficients.resize (4);
96 model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1];
97 model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2];
98 model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0];
99 model_coefficients[3] = 0;
102 model_coefficients.normalize ();
105 model_coefficients[3] = -1 * (model_coefficients.template head<4>().dot (p0.matrix ()));
111 template <
typename Po
intT>
void
113 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
116 if (model_coefficients.size () != model_size_)
118 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::getDistancesToModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
122 distances.resize (indices_->size ());
125 for (
size_t i = 0; i < indices_->size (); ++i)
133 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
134 input_->points[(*indices_)[i]].y,
135 input_->points[(*indices_)[i]].z,
137 distances[i] = fabs (model_coefficients.dot (pt));
142 template <
typename Po
intT>
void
144 const Eigen::VectorXf &model_coefficients,
const double threshold, std::vector<int> &inliers)
147 if (model_coefficients.size () != model_size_)
149 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
154 inliers.resize (indices_->size ());
155 error_sqr_dists_.resize (indices_->size ());
158 for (
size_t i = 0; i < indices_->size (); ++i)
162 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
163 input_->points[(*indices_)[i]].y,
164 input_->points[(*indices_)[i]].z,
167 float distance = fabsf (model_coefficients.dot (pt));
169 if (distance < threshold)
172 inliers[nr_p] = (*indices_)[i];
173 error_sqr_dists_[nr_p] =
static_cast<double> (distance);
177 inliers.resize (nr_p);
178 error_sqr_dists_.resize (nr_p);
182 template <
typename Po
intT>
int
184 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
187 if (model_coefficients.size () != model_size_)
189 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::countWithinDistance] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
196 for (
size_t i = 0; i < indices_->size (); ++i)
200 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
201 input_->points[(*indices_)[i]].y,
202 input_->points[(*indices_)[i]].z,
204 if (fabs (model_coefficients.dot (pt)) < threshold)
211 template <
typename Po
intT>
void
213 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
216 if (model_coefficients.size () != model_size_)
218 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
219 optimized_coefficients = model_coefficients;
224 if (inliers.size () <= sample_size_)
226 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
227 optimized_coefficients = model_coefficients;
231 Eigen::Vector4f plane_parameters;
235 Eigen::Vector4f xyz_centroid;
242 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
245 optimized_coefficients.resize (4);
246 optimized_coefficients[0] = eigen_vector [0];
247 optimized_coefficients[1] = eigen_vector [1];
248 optimized_coefficients[2] = eigen_vector [2];
249 optimized_coefficients[3] = 0;
250 optimized_coefficients[3] = -1 * optimized_coefficients.dot (xyz_centroid);
253 if (!isModelValid (optimized_coefficients))
255 optimized_coefficients = model_coefficients;
260 template <
typename Po
intT>
void
262 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const
265 if (model_coefficients.size () != model_size_)
267 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
271 projected_points.
header = input_->header;
272 projected_points.
is_dense = input_->is_dense;
274 Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
279 Eigen::Vector4f tmp_mc = model_coefficients;
285 if (copy_data_fields)
288 projected_points.
points.resize (input_->points.size ());
289 projected_points.
width = input_->width;
290 projected_points.
height = input_->height;
294 for (
size_t i = 0; i < input_->points.size (); ++i)
299 for (
size_t i = 0; i < inliers.size (); ++i)
302 Eigen::Vector4f p (input_->points[inliers[i]].x,
303 input_->points[inliers[i]].y,
304 input_->points[inliers[i]].z,
307 float distance_to_plane = tmp_mc.dot (p);
310 pp.matrix () = p - mc * distance_to_plane;
316 projected_points.
points.resize (inliers.size ());
317 projected_points.
width =
static_cast<uint32_t
> (inliers.size ());
318 projected_points.
height = 1;
322 for (
size_t i = 0; i < inliers.size (); ++i)
327 for (
size_t i = 0; i < inliers.size (); ++i)
330 Eigen::Vector4f p (input_->points[inliers[i]].x,
331 input_->points[inliers[i]].y,
332 input_->points[inliers[i]].z,
335 float distance_to_plane = tmp_mc.dot (p);
338 pp.matrix () = p - mc * distance_to_plane;
344 template <
typename Po
intT>
bool
346 const std::set<int> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
349 if (model_coefficients.size () != model_size_)
351 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
355 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
357 Eigen::Vector4f pt (input_->points[*it].x,
358 input_->points[*it].y,
359 input_->points[*it].z,
361 if (fabs (model_coefficients.dot (pt)) > threshold)
368 #define PCL_INSTANTIATE_SampleConsensusModelPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelPlane<T>;
370 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
uint32_t width
The point cloud width (if organized as an image-structure).
struct pcl::PointXYZIEdge EIGEN_ALIGN16
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
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.
void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const
Create a new point cloud with inliers projected onto the plane model.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
uint32_t height
The point cloud height (if organized as an image-structure).
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
Recompute the plane coefficients using the given inlier set and return them to the user...
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the cloud data to a given plane model.
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients) const
Check whether the given index samples can form a valid plane model, compute the model coefficients fr...
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
Verify whether a subset of indices verifies the given plane model coefficients.
Helper functor structure for concatenate.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
SampleConsensusModelPlane defines a model for 3D plane segmentation.
pcl::PCLHeader header
The point cloud header.