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
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)
const
73 if (samples.size () != sample_size_)
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)
const
116 if (model_coefficients.size () != model_size_)
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 () != model_size_)
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));
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)
const
187 if (model_coefficients.size () != model_size_)
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)
const
216 if (model_coefficients.size () != model_size_)
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 () <= sample_size_)
226 PCL_ERROR (
"[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%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)
const
265 if (model_coefficients.size () != model_size_)
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)
const
349 if (model_coefficients.size () != model_size_)
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_