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)
73 if (samples.size () != 3)
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)
116 if (model_coefficients.size () != 4)
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 () != 4)
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)
187 if (model_coefficients.size () != 4)
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)
216 if (model_coefficients.size () != 4)
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 () < 4)
226 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to support a model (%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)
265 if (model_coefficients.size () != 4)
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)
349 if (model_coefficients.size () != 4)
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::_PointXYZHSV 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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true)
Create a new point cloud with inliers projected onto the plane model.
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)
Check whether the given index samples can form a valid plane model, compute the model coefficients fr...
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the cloud data to a given plane model.
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)
Recompute the plane coefficients using the given inlier set and return them to the user...
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
Helper functor structure for concatenate.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
SampleConsensusModelPlane defines a model for 3D plane segmentation.
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
Verify whether a subset of indices verifies the given plane model coefficients.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
pcl::PCLHeader header
The point cloud header.