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
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));
174 if (distance < threshold)
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 ());
321 projected_points.
width =
static_cast<std::uint32_t
> (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>;
378 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_
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...
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...
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.
std::uint32_t width
The point cloud width (if organized as an image-structure).
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...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
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. ...
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
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.
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.
std::uint32_t height
The point cloud height (if organized as an image-structure).
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.
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
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.
pcl::PCLHeader header
The point cloud header.