53 if (samples.size () != sample_size_)
55 PCL_ERROR (
"[pcl::SampleConsensusModelStick::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
59 ((*input_)[samples[0]].x != (*input_)[samples[1]].x)
61 ((*input_)[samples[0]].y != (*input_)[samples[1]].y)
63 ((*input_)[samples[0]].z != (*input_)[samples[1]].z))
74 const Indices &samples, Eigen::VectorXf &model_coefficients)
const
77 if (samples.size () != sample_size_)
79 PCL_ERROR (
"[pcl::SampleConsensusModelStick::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
83 model_coefficients.resize (model_size_);
84 model_coefficients[0] = (*input_)[samples[0]].x;
85 model_coefficients[1] = (*input_)[samples[0]].y;
86 model_coefficients[2] = (*input_)[samples[0]].z;
88 model_coefficients[3] = (*input_)[samples[1]].x;
89 model_coefficients[4] = (*input_)[samples[1]].y;
90 model_coefficients[5] = (*input_)[samples[1]].z;
99 PCL_DEBUG (
"[pcl::SampleConsensusModelStick::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g).\n",
100 model_coefficients[0], model_coefficients[1], model_coefficients[2],
101 model_coefficients[3], model_coefficients[4], model_coefficients[5]);
108 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
111 if (!isModelValid (model_coefficients))
113 PCL_ERROR (
"[pcl::SampleConsensusModelStick::getDistancesToModel] Given model is invalid!\n");
117 float sqr_threshold =
static_cast<float> (radius_max_ * radius_max_);
118 distances.resize (indices_->size ());
121 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
122 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
123 line_dir.normalize ();
126 for (std::size_t i = 0; i < indices_->size (); ++i)
130 float sqr_distance = (line_pt - (*input_)[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
132 if (sqr_distance < sqr_threshold)
135 distances[i] = std::sqrt (sqr_distance);
140 distances[i] = 2 * std::sqrt (sqr_distance);
148 const Eigen::VectorXf &model_coefficients,
const double threshold,
Indices &inliers)
151 if (!isModelValid (model_coefficients))
153 PCL_ERROR (
"[pcl::SampleConsensusModelStick::selectWithinDistance] Given model is invalid!\n");
157 float sqr_threshold =
static_cast<float> (threshold * threshold);
160 error_sqr_dists_.clear ();
161 inliers.reserve (indices_->size ());
162 error_sqr_dists_.reserve (indices_->size ());
165 Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
166 Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
167 Eigen::Vector4f line_dir = line_pt2 - line_pt1;
170 line_dir.normalize ();
174 for (std::size_t i = 0; i < indices_->size (); ++i)
178 Eigen::Vector4f dir = (*input_)[(*indices_)[i]].getVector4fMap () - line_pt1;
185 float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
186 if (sqr_distance < sqr_threshold)
189 inliers.push_back ((*indices_)[i]);
190 error_sqr_dists_.push_back (
static_cast<double> (sqr_distance));
198 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
201 if (!isModelValid (model_coefficients))
203 PCL_ERROR (
"[pcl::SampleConsensusModelStick::countWithinDistance] Given model is invalid!\n");
207 float sqr_threshold =
static_cast<float> (threshold * threshold);
209 std::size_t nr_i = 0, nr_o = 0;
212 Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
213 Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
214 Eigen::Vector4f line_dir = line_pt2 - line_pt1;
215 line_dir.normalize ();
221 for (std::size_t i = 0; i < indices_->size (); ++i)
225 Eigen::Vector4f dir = (*input_)[(*indices_)[i]].getVector4fMap () - line_pt1;
232 float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
234 if (sqr_distance < sqr_threshold)
238 else if (sqr_distance < 4.0f * sqr_threshold)
244 return (nr_i <= nr_o ? 0 : nr_i - nr_o);
250 const Indices &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
253 if (!isModelValid (model_coefficients))
255 optimized_coefficients = model_coefficients;
260 if (inliers.size () <= sample_size_)
262 PCL_ERROR (
"[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
263 optimized_coefficients = model_coefficients;
267 optimized_coefficients.resize (model_size_);
270 Eigen::Vector4f centroid;
271 Eigen::Matrix3f covariance_matrix;
275 PCL_ERROR (
"[pcl::SampleConsensusModelStick::optimizeModelCoefficients] computeMeanAndCovarianceMatrix failed (returned 0) because there are no valid inliers.\n");
276 optimized_coefficients = model_coefficients;
280 optimized_coefficients[0] = centroid[0];
281 optimized_coefficients[1] = centroid[1];
282 optimized_coefficients[2] = centroid[2];
285 Eigen::Vector3f eigen_values;
286 Eigen::Vector3f eigen_vector;
290 optimized_coefficients.template segment<3> (3).matrix () = eigen_vector;
296 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const
299 if (!isModelValid (model_coefficients))
301 PCL_ERROR (
"[pcl::SampleConsensusModelStick::projectPoints] Given model is invalid!\n");
306 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
307 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
309 projected_points.header = input_->header;
310 projected_points.is_dense = input_->is_dense;
313 if (copy_data_fields)
316 projected_points.resize (input_->size ());
317 projected_points.width = input_->width;
318 projected_points.height = input_->height;
320 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
322 for (std::size_t i = 0; i < projected_points.size (); ++i)
325 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[i], projected_points[i]));
329 for (
const auto &inlier : inliers)
331 Eigen::Vector4f pt ((*input_)[inlier].x, (*input_)[inlier].y, (*input_)[inlier].z, 0.0f);
333 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
335 Eigen::Vector4f pp = line_pt + k * line_dir;
337 projected_points[inlier].x = pp[0];
338 projected_points[inlier].y = pp[1];
339 projected_points[inlier].z = pp[2];
345 projected_points.resize (inliers.size ());
346 projected_points.width = inliers.size ();
347 projected_points.height = 1;
349 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
351 for (std::size_t i = 0; i < inliers.size (); ++i)
354 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[inliers[i]], projected_points[i]));
358 for (std::size_t i = 0; i < inliers.size (); ++i)
360 Eigen::Vector4f pt ((*input_)[inliers[i]].x, (*input_)[inliers[i]].y, (*input_)[inliers[i]].z, 0.0f);
362 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
364 Eigen::Vector4f pp = line_pt + k * line_dir;
366 projected_points[i].x = pp[0];
367 projected_points[i].y = pp[1];
368 projected_points[i].z = pp[2];
376 const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
379 if (!isModelValid (model_coefficients))
381 PCL_ERROR (
"[pcl::SampleConsensusModelStick::doSamplesVerifyModel] Given model is invalid!\n");
386 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
387 Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0.0f);
389 line_dir.normalize ();
391 float sqr_threshold =
static_cast<float> (threshold * threshold);
393 for (
const auto &index : indices)
397 if ((line_pt - (*input_)[index].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold)
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 stick model.
void computeCorrespondingEigenVector(const Matrix &mat, const typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the corresponding eigenvector to the given eigenvalue of the symmetric positive semi defin...
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 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...