41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_ 42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_ 44 #include <pcl/sample_consensus/sac_model_stick.h> 46 #include <pcl/common/concatenate.h> 49 template <
typename Po
intT>
bool 52 if (samples.size () != sample_size_)
54 PCL_ERROR (
"[pcl::SampleConsensusModelStick::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
58 ((*input_)[samples[0]].x != (*input_)[samples[1]].x)
60 ((*input_)[samples[0]].y != (*input_)[samples[1]].y)
62 ((*input_)[samples[0]].z != (*input_)[samples[1]].z))
71 template <
typename Po
intT>
bool 73 const Indices &samples, Eigen::VectorXf &model_coefficients)
const 76 if (samples.size () != sample_size_)
78 PCL_ERROR (
"[pcl::SampleConsensusModelStick::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
82 model_coefficients.resize (model_size_);
83 model_coefficients[0] = (*input_)[samples[0]].x;
84 model_coefficients[1] = (*input_)[samples[0]].y;
85 model_coefficients[2] = (*input_)[samples[0]].z;
87 model_coefficients[3] = (*input_)[samples[1]].x;
88 model_coefficients[4] = (*input_)[samples[1]].y;
89 model_coefficients[5] = (*input_)[samples[1]].z;
102 template <
typename Po
intT>
void 104 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const 107 if (!isModelValid (model_coefficients))
109 PCL_ERROR (
"[pcl::SampleConsensusModelStick::getDistancesToModel] Given model is invalid!\n");
113 float sqr_threshold =
static_cast<float> (radius_max_ * radius_max_);
114 distances.resize (indices_->size ());
117 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
118 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
119 line_dir.normalize ();
122 for (std::size_t i = 0; i < indices_->size (); ++i)
126 float sqr_distance = (line_pt - (*input_)[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
128 if (sqr_distance < sqr_threshold)
131 distances[i] = sqrt (sqr_distance);
136 distances[i] = 2 * sqrt (sqr_distance);
142 template <
typename Po
intT>
void 144 const Eigen::VectorXf &model_coefficients,
const double threshold,
Indices &inliers)
147 if (!isModelValid (model_coefficients))
149 PCL_ERROR (
"[pcl::SampleConsensusModelStick::selectWithinDistance] Given model is invalid!\n");
153 float sqr_threshold =
static_cast<float> (threshold * threshold);
156 error_sqr_dists_.clear ();
157 inliers.reserve (indices_->size ());
158 error_sqr_dists_.reserve (indices_->size ());
161 Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
162 Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
163 Eigen::Vector4f line_dir = line_pt2 - line_pt1;
166 line_dir.normalize ();
170 for (std::size_t i = 0; i < indices_->size (); ++i)
174 Eigen::Vector4f dir = (*input_)[(*indices_)[i]].getVector4fMap () - line_pt1;
181 float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
182 if (sqr_distance < sqr_threshold)
185 inliers.push_back ((*indices_)[i]);
186 error_sqr_dists_.push_back (static_cast<double> (sqr_distance));
192 template <
typename Po
intT> std::size_t
194 const Eigen::VectorXf &model_coefficients,
const double threshold)
const 197 if (!isModelValid (model_coefficients))
199 PCL_ERROR (
"[pcl::SampleConsensusModelStick::countWithinDistance] Given model is invalid!\n");
203 float sqr_threshold =
static_cast<float> (threshold * threshold);
205 std::size_t nr_i = 0, nr_o = 0;
208 Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
209 Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
210 Eigen::Vector4f line_dir = line_pt2 - line_pt1;
211 line_dir.normalize ();
217 for (std::size_t i = 0; i < indices_->size (); ++i)
221 Eigen::Vector4f dir = (*input_)[(*indices_)[i]].getVector4fMap () - line_pt1;
228 float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
230 if (sqr_distance < sqr_threshold)
234 else if (sqr_distance < 4.0f * sqr_threshold)
240 return (nr_i <= nr_o ? 0 : nr_i - nr_o);
244 template <
typename Po
intT>
void 246 const Indices &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const 249 if (!isModelValid (model_coefficients))
251 optimized_coefficients = model_coefficients;
256 if (inliers.size () <= sample_size_)
258 PCL_ERROR (
"[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
259 optimized_coefficients = model_coefficients;
263 optimized_coefficients.resize (model_size_);
266 Eigen::Vector4f centroid;
267 Eigen::Matrix3f covariance_matrix;
271 optimized_coefficients[0] = centroid[0];
272 optimized_coefficients[1] = centroid[1];
273 optimized_coefficients[2] = centroid[2];
276 Eigen::Vector3f eigen_values;
277 Eigen::Vector3f eigen_vector;
281 optimized_coefficients.template segment<3> (3).matrix () = eigen_vector;
285 template <
typename Po
intT>
void 287 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const 290 if (!isModelValid (model_coefficients))
292 PCL_ERROR (
"[pcl::SampleConsensusModelStick::projectPoints] Given model is invalid!\n");
297 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
298 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
300 projected_points.
header = input_->header;
301 projected_points.
is_dense = input_->is_dense;
304 if (copy_data_fields)
307 projected_points.
points.resize (input_->size ());
308 projected_points.
width = input_->width;
309 projected_points.
height = input_->height;
311 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
313 for (std::size_t i = 0; i < projected_points.
size (); ++i)
320 for (
const auto &inlier : inliers)
322 Eigen::Vector4f pt ((*input_)[inlier].x, (*input_)[inlier].y, (*input_)[inlier].z, 0.0f);
324 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
326 Eigen::Vector4f pp = line_pt + k * line_dir;
328 projected_points[inlier].x = pp[0];
329 projected_points[inlier].y = pp[1];
330 projected_points[inlier].z = pp[2];
336 projected_points.
points.resize (inliers.size ());
337 projected_points.
width = inliers.size ();
338 projected_points.
height = 1;
340 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
342 for (std::size_t i = 0; i < inliers.size (); ++i)
349 for (std::size_t i = 0; i < inliers.size (); ++i)
351 Eigen::Vector4f pt ((*input_)[inliers[i]].x, (*input_)[inliers[i]].y, (*input_)[inliers[i]].z, 0.0f);
353 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
355 Eigen::Vector4f pp = line_pt + k * line_dir;
357 projected_points[i].x = pp[0];
358 projected_points[i].y = pp[1];
359 projected_points[i].z = pp[2];
365 template <
typename Po
intT>
bool 367 const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const 370 if (!isModelValid (model_coefficients))
372 PCL_ERROR (
"[pcl::SampleConsensusModelStick::doSamplesVerifyModel] Given model is invalid!\n");
377 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
378 Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0.0f);
380 line_dir.normalize ();
382 float sqr_threshold =
static_cast<float> (threshold * threshold);
384 for (
const auto &index : indices)
388 if ((line_pt - (*input_)[index].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold)
397 #define PCL_INSTANTIATE_SampleConsensusModelStick(T) template class PCL_EXPORTS pcl::SampleConsensusModelStick<T>; 399 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_ void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all squared distances from the cloud data to a given 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...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid stick model, compute the model coefficients fr...
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
std::uint32_t width
The point cloud width (if organized as an image-structure).
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.
std::uint32_t height
The point cloud height (if organized as an image-structure).
IndicesAllocator<> Indices
Type used for indices in PCL.
bool doSamplesVerifyModel(const std::set< index_t > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override
Verify whether a subset of indices verifies the given stick model coefficients.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
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...
std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const override
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 in any of their floating point fields)...
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the stick coefficients using the given inlier set and return them to the user...
Helper functor structure for concatenate.
Define methods for centroid estimation and covariance matrix calculus.