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>
46 #include <pcl/common/eigen.h>
47 #include <pcl/common/concatenate.h>
51 template <
typename Po
intT>
bool
54 if (samples.size () != sample_size_)
56 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
64 Eigen::Array4f dy1dy2 = (p1-p0) / (p2-p0);
66 return ( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) );
70 template <
typename Po
intT>
bool
72 const Indices &samples, Eigen::VectorXf &model_coefficients)
const
75 if (samples.size () != sample_size_)
77 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
86 Eigen::Array4f p1p0 = p1 - p0;
88 Eigen::Array4f p2p0 = p2 - p0;
91 Eigen::Array4f dy1dy2 = p1p0 / p2p0;
92 if ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) )
99 model_coefficients.resize (model_size_);
100 model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1];
101 model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2];
102 model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0];
103 model_coefficients[3] = 0.0f;
106 model_coefficients.normalize ();
109 model_coefficients[3] = -1.0f * (model_coefficients.template head<4>().dot (p0.matrix ()));
115 template <
typename Po
intT>
void
117 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
120 if (!isModelValid (model_coefficients))
122 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::getDistancesToModel] Given model is invalid!\n");
126 distances.resize (indices_->size ());
129 for (std::size_t i = 0; i < indices_->size (); ++i)
137 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
138 input_->points[(*indices_)[i]].y,
139 input_->points[(*indices_)[i]].z,
141 distances[i] = std::abs (model_coefficients.dot (pt));
146 template <
typename Po
intT>
void
148 const Eigen::VectorXf &model_coefficients,
const double threshold,
Indices &inliers)
151 if (!isModelValid (model_coefficients))
153 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::selectWithinDistance] Given model is invalid!\n");
158 error_sqr_dists_.clear ();
159 inliers.reserve (indices_->size ());
160 error_sqr_dists_.reserve (indices_->size ());
163 for (std::size_t i = 0; i < indices_->size (); ++i)
167 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
168 input_->points[(*indices_)[i]].y,
169 input_->points[(*indices_)[i]].z,
172 float distance = std::abs (model_coefficients.dot (pt));
177 inliers.push_back ((*indices_)[i]);
178 error_sqr_dists_.push_back (
static_cast<double> (
distance));
184 template <
typename Po
intT> std::size_t
186 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
189 if (!isModelValid (model_coefficients))
191 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::countWithinDistance] Given model is invalid!\n");
195 std::size_t nr_p = 0;
198 for (std::size_t i = 0; i < indices_->size (); ++i)
202 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
203 input_->points[(*indices_)[i]].y,
204 input_->points[(*indices_)[i]].z,
206 if (std::abs (model_coefficients.dot (pt)) < threshold)
215 template <
typename Po
intT>
void
217 const Indices &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
220 if (!isModelValid (model_coefficients))
222 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Given model is invalid!\n");
223 optimized_coefficients = model_coefficients;
228 if (inliers.size () <= sample_size_)
230 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
231 optimized_coefficients = model_coefficients;
235 Eigen::Vector4f plane_parameters;
238 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
239 Eigen::Vector4f xyz_centroid;
244 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
245 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
246 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
249 optimized_coefficients.resize (model_size_);
250 optimized_coefficients[0] = eigen_vector [0];
251 optimized_coefficients[1] = eigen_vector [1];
252 optimized_coefficients[2] = eigen_vector [2];
253 optimized_coefficients[3] = 0.0f;
254 optimized_coefficients[3] = -1.0f * optimized_coefficients.dot (xyz_centroid);
257 if (!isModelValid (optimized_coefficients))
259 optimized_coefficients = model_coefficients;
264 template <
typename Po
intT>
void
266 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const
269 if (!isModelValid (model_coefficients))
271 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::projectPoints] Given model is invalid!\n");
275 projected_points.
header = input_->header;
276 projected_points.
is_dense = input_->is_dense;
278 Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
283 Eigen::Vector4f tmp_mc = model_coefficients;
289 if (copy_data_fields)
292 projected_points.
points.resize (input_->points.size ());
293 projected_points.
width = input_->width;
294 projected_points.
height = input_->height;
298 for (std::size_t i = 0; i < input_->points.size (); ++i)
303 for (
const auto &inlier : inliers)
306 Eigen::Vector4f p (input_->points[inlier].x,
307 input_->points[inlier].y,
308 input_->points[inlier].z,
311 float distance_to_plane = tmp_mc.dot (p);
314 pp.matrix () = p - mc * distance_to_plane;
320 projected_points.
points.resize (inliers.size ());
322 projected_points.
height = 1;
326 for (std::size_t i = 0; i < inliers.size (); ++i)
333 for (std::size_t i = 0; i < inliers.size (); ++i)
336 Eigen::Vector4f p (input_->points[inliers[i]].x,
337 input_->points[inliers[i]].y,
338 input_->points[inliers[i]].z,
341 float distance_to_plane = tmp_mc.dot (p);
344 pp.matrix () = p - mc * distance_to_plane;
350 template <
typename Po
intT>
bool
352 const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
355 if (!isModelValid (model_coefficients))
357 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Given model is invalid!\n");
361 for (
const auto &index : indices)
363 Eigen::Vector4f pt (input_->points[index].x,
364 input_->points[index].y,
365 input_->points[index].z,
367 if (std::abs (model_coefficients.dot (pt)) > threshold)
376 #define PCL_INSTANTIATE_SampleConsensusModelPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelPlane<T>;
Define methods for centroid estimation and covariance matrix calculus.
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
SampleConsensusModelPlane defines a model for 3D plane segmentation.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
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 override
Compute all distances from the cloud data to a given plane model.
bool doSamplesVerifyModel(const std::set< index_t > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override
Verify whether a subset of indices verifies the given plane model coefficients.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
void projectPoints(const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const override
Create a new point cloud with inliers projected onto the plane model.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid plane model, compute the model coefficients fr...
std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const override
Count all the points which respect the given model coefficients as inliers.
Defines all the PCL implemented PointT point type structures.
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 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...
float distance(const PointT &p1, const PointT &p2)
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
std::vector< index_t > Indices
Type used for indices in PCL.
Helper functor structure for concatenate.