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>
50 template <
typename Po
intT>
bool
53 if (samples.size () != sample_size_)
55 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
63 Eigen::Array4f dy1dy2 = (p1-p0) / (p2-p0);
65 return ( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) );
69 template <
typename Po
intT>
bool
71 const Indices &samples, Eigen::VectorXf &model_coefficients)
const
74 if (samples.size () != sample_size_)
76 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
85 Eigen::Array4f p1p0 = p1 - p0;
87 Eigen::Array4f p2p0 = p2 - p0;
90 Eigen::Array4f dy1dy2 = p1p0 / p2p0;
91 if ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) )
98 model_coefficients.resize (model_size_);
99 model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1];
100 model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2];
101 model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0];
102 model_coefficients[3] = 0.0f;
105 model_coefficients.normalize ();
108 model_coefficients[3] = -1.0f * (model_coefficients.template head<4>().dot (p0.matrix ()));
114 template <
typename Po
intT>
void
116 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
119 if (!isModelValid (model_coefficients))
121 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::getDistancesToModel] Given model is invalid!\n");
125 distances.resize (indices_->size ());
128 for (std::size_t i = 0; i < indices_->size (); ++i)
136 Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
137 (*input_)[(*indices_)[i]].y,
138 (*input_)[(*indices_)[i]].z,
140 distances[i] = std::abs (model_coefficients.dot (pt));
145 template <
typename Po
intT>
void
147 const Eigen::VectorXf &model_coefficients,
const double threshold,
Indices &inliers)
150 if (!isModelValid (model_coefficients))
152 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::selectWithinDistance] Given model is invalid!\n");
157 error_sqr_dists_.clear ();
158 inliers.reserve (indices_->size ());
159 error_sqr_dists_.reserve (indices_->size ());
162 for (std::size_t i = 0; i < indices_->size (); ++i)
166 Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
167 (*input_)[(*indices_)[i]].y,
168 (*input_)[(*indices_)[i]].z,
171 float distance = std::abs (model_coefficients.dot (pt));
176 inliers.push_back ((*indices_)[i]);
177 error_sqr_dists_.push_back (
static_cast<double> (
distance));
183 template <
typename Po
intT> std::size_t
185 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
188 if (!isModelValid (model_coefficients))
190 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::countWithinDistance] Given model is invalid!\n");
194 std::size_t nr_p = 0;
197 for (std::size_t i = 0; i < indices_->size (); ++i)
201 Eigen::Vector4f pt ((*input_)[(*indices_)[i]].x,
202 (*input_)[(*indices_)[i]].y,
203 (*input_)[(*indices_)[i]].z,
205 if (std::abs (model_coefficients.dot (pt)) < threshold)
214 template <
typename Po
intT>
void
216 const Indices &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
219 if (!isModelValid (model_coefficients))
221 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Given model is invalid!\n");
222 optimized_coefficients = model_coefficients;
227 if (inliers.size () <= sample_size_)
229 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
230 optimized_coefficients = model_coefficients;
234 Eigen::Vector4f plane_parameters;
237 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
238 Eigen::Vector4f xyz_centroid;
243 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
244 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
245 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
248 optimized_coefficients.resize (model_size_);
249 optimized_coefficients[0] = eigen_vector [0];
250 optimized_coefficients[1] = eigen_vector [1];
251 optimized_coefficients[2] = eigen_vector [2];
252 optimized_coefficients[3] = 0.0f;
253 optimized_coefficients[3] = -1.0f * optimized_coefficients.dot (xyz_centroid);
256 if (!isModelValid (optimized_coefficients))
258 optimized_coefficients = model_coefficients;
263 template <
typename Po
intT>
void
265 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const
268 if (!isModelValid (model_coefficients))
270 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::projectPoints] Given model is invalid!\n");
274 projected_points.
header = input_->header;
275 projected_points.
is_dense = input_->is_dense;
277 Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
282 Eigen::Vector4f tmp_mc = model_coefficients;
288 if (copy_data_fields)
291 projected_points.
points.resize (input_->size ());
292 projected_points.
width = input_->width;
293 projected_points.
height = input_->height;
295 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
297 for (std::size_t i = 0; i < input_->size (); ++i)
302 for (
const auto &inlier : inliers)
305 Eigen::Vector4f p ((*input_)[inlier].x,
310 float distance_to_plane = tmp_mc.dot (p);
313 pp.matrix () = p - mc * distance_to_plane;
319 projected_points.
points.resize (inliers.size ());
320 projected_points.
width = inliers.size ();
321 projected_points.
height = 1;
323 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
325 for (std::size_t i = 0; i < inliers.size (); ++i)
332 for (std::size_t i = 0; i < inliers.size (); ++i)
335 Eigen::Vector4f p ((*input_)[inliers[i]].x,
336 (*input_)[inliers[i]].y,
337 (*input_)[inliers[i]].z,
340 float distance_to_plane = tmp_mc.dot (p);
343 pp.matrix () = p - mc * distance_to_plane;
349 template <
typename Po
intT>
bool
351 const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
354 if (!isModelValid (model_coefficients))
356 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Given model is invalid!\n");
360 for (
const auto &index : indices)
362 Eigen::Vector4f pt ((*input_)[index].x,
366 if (std::abs (model_coefficients.dot (pt)) > threshold)
375 #define PCL_INSTANTIATE_SampleConsensusModelPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelPlane<T>;
377 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_