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
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.
uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
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.
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...
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.
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
uint32_t height
The point cloud height (if organized as an image-structure).